From c1c17c063300193ed06279e04886a683b44910c9 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Sat, 2 Jul 2016 21:34:27 -0300 Subject: [PATCH] Extracted the Ruby bindings for Eigen in a separate package, bound to be released as a gem --- .gitignore | 14 + .travis.yml | 14 + Gemfile | 4 + LICENSE.txt | 21 ++ README.md | 41 +++ Rakefile | 18 ++ bin/console | 14 + bin/setup | 8 + eigen.gemspec | 27 ++ ext/eigen/eigen.cpp | 675 ++++++++++++++++++++++++++++++++++++++++ ext/eigen/extconf.rb | 10 + lib/eigen.rb | 15 + lib/eigen/affine3.rb | 35 +++ lib/eigen/angle_axis.rb | 90 ++++++ lib/eigen/isometry3.rb | 36 +++ lib/eigen/matrix4.rb | 73 +++++ lib/eigen/matrixx.rb | 95 ++++++ lib/eigen/quaternion.rb | 253 +++++++++++++++ lib/eigen/vector3.rb | 178 +++++++++++ lib/eigen/vectorx.rb | 54 ++++ lib/eigen/version.rb | 3 + test/affine3_test.rb | 79 +++++ test/angleaxis_test.rb | 76 +++++ test/isometry3_test.rb | 79 +++++ test/matrixx_test.rb | 125 ++++++++ test/quaternion_test.rb | 86 +++++ test/test_helper.rb | 54 ++++ test/vector3_test.rb | 80 +++++ 28 files changed, 2257 insertions(+) create mode 100644 .gitignore create mode 100644 .travis.yml create mode 100644 Gemfile create mode 100644 LICENSE.txt create mode 100644 README.md create mode 100644 Rakefile create mode 100755 bin/console create mode 100755 bin/setup create mode 100644 eigen.gemspec create mode 100644 ext/eigen/eigen.cpp create mode 100644 ext/eigen/extconf.rb create mode 100644 lib/eigen.rb create mode 100644 lib/eigen/affine3.rb create mode 100644 lib/eigen/angle_axis.rb create mode 100644 lib/eigen/isometry3.rb create mode 100644 lib/eigen/matrix4.rb create mode 100644 lib/eigen/matrixx.rb create mode 100644 lib/eigen/quaternion.rb create mode 100644 lib/eigen/vector3.rb create mode 100644 lib/eigen/vectorx.rb create mode 100644 lib/eigen/version.rb create mode 100644 test/affine3_test.rb create mode 100644 test/angleaxis_test.rb create mode 100644 test/isometry3_test.rb create mode 100644 test/matrixx_test.rb create mode 100644 test/quaternion_test.rb create mode 100644 test/test_helper.rb create mode 100644 test/vector3_test.rb diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ae3fdc2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,14 @@ +/.bundle/ +/.yardoc +/Gemfile.lock +/_yardoc/ +/coverage/ +/doc/ +/pkg/ +/spec/reports/ +/tmp/ +*.bundle +*.so +*.o +*.a +mkmf.log diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..33225aa --- /dev/null +++ b/.travis.yml @@ -0,0 +1,14 @@ +sudo: false +language: ruby +rvm: + - 2.1.9 + - 2.2.5 + - 2.3.1 +before_install: gem install bundler -v 1.12.1 +addons: + apt: + packages: libeigen3-dev +script: + - rake + - rake test + diff --git a/Gemfile b/Gemfile new file mode 100644 index 0000000..f13c98a --- /dev/null +++ b/Gemfile @@ -0,0 +1,4 @@ +source 'https://rubygems.org' + +# Specify your gem's dependencies in eigen.gemspec +gemspec diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..142429e --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2016 Sylvain Joyeux + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..2389e86 --- /dev/null +++ b/README.md @@ -0,0 +1,41 @@ +# Eigen + +This gem provides bindings for the +[Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) linear algebra +library. They are quite limited, to the methods / functionality that were +needed. Functionality gets added as it goes, there is no goal to have a 100% +coverage of the Eigen functionality. + +## Installation + +Add this line to your application's Gemfile: + +```ruby +gem 'eigen' +``` + +And then execute: + + $ bundle + +Or install it yourself as: + + $ gem install eigen + +## Usage + +## Development + +After checking out the repo, run `bin/setup` to install dependencies. Then, run `rake test` to run the tests. You can also run `bin/console` for an interactive prompt that will allow you to experiment. + +To install this gem onto your local machine, run `bundle exec rake install`. To release a new version, update the version number in `version.rb`, and then run `bundle exec rake release`, which will create a git tag for the version, push git commits and tags, and push the `.gem` file to [rubygems.org](https://rubygems.org). + +## Contributing + +Bug reports and pull requests are welcome on GitHub at https://github.com/rock-core/base-ruby_eigen + + +## License + +The gem is available as open source under the terms of the [MIT License](http://opensource.org/licenses/MIT). + diff --git a/Rakefile b/Rakefile new file mode 100644 index 0000000..ea53039 --- /dev/null +++ b/Rakefile @@ -0,0 +1,18 @@ +require "bundler/gem_tasks" +require "rake/testtask" + +Rake::TestTask.new(:test) do |t| + t.libs << "test" + t.libs << "lib" + t.test_files = FileList['test/**/*_test.rb'] +end + +require "rake/extensiontask" + +task :build => :compile + +Rake::ExtensionTask.new("eigen") do |ext| + ext.lib_dir = "lib/eigen" +end + +task :default => :compile diff --git a/bin/console b/bin/console new file mode 100755 index 0000000..8076637 --- /dev/null +++ b/bin/console @@ -0,0 +1,14 @@ +#!/usr/bin/env ruby + +require "bundler/setup" +require "eigen" + +# You can add fixtures and/or initialization code here to make experimenting +# with your gem easier. You can also use a different console, if you like. + +# (If you use this, don't forget to add pry to your Gemfile!) +# require "pry" +# Pry.start + +require "irb" +IRB.start diff --git a/bin/setup b/bin/setup new file mode 100755 index 0000000..dce67d8 --- /dev/null +++ b/bin/setup @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -euo pipefail +IFS=$'\n\t' +set -vx + +bundle install + +# Do any other automated setup that you need to do here diff --git a/eigen.gemspec b/eigen.gemspec new file mode 100644 index 0000000..42e2cbe --- /dev/null +++ b/eigen.gemspec @@ -0,0 +1,27 @@ +# coding: utf-8 +lib = File.expand_path('../lib', __FILE__) +$LOAD_PATH.unshift(lib) unless $LOAD_PATH.include?(lib) +require 'eigen/version' + +Gem::Specification.new do |spec| + spec.name = "eigen" + spec.version = Eigen::VERSION + spec.authors = ["Sylvain Joyeux"] + spec.email = ["sylvain.joyeux@m4x.org"] + + spec.summary = %q{Ruby bindings to the Eigen C++ linear algebra library} + spec.homepage = "https://github.com/rock-core/base-ruby_eigen" + spec.license = "MIT" + + spec.files = `git ls-files -z`.split("\x0").reject { |f| f.match(%r{^(test|spec|features)/}) } + spec.bindir = "exe" + spec.executables = spec.files.grep(%r{^exe/}) { |f| File.basename(f) } + spec.require_paths = ["lib"] + spec.extensions = ["ext/eigen/extconf.rb"] + + spec.add_development_dependency 'rice', '~> 2.1', '>= 2.1.0' + spec.add_development_dependency "bundler", "~> 1.12" + spec.add_development_dependency "rake", "~> 10.0" + spec.add_development_dependency "rake-compiler" + spec.add_development_dependency "minitest", "~> 5.0" +end diff --git a/ext/eigen/eigen.cpp b/ext/eigen/eigen.cpp new file mode 100644 index 0000000..6f20c80 --- /dev/null +++ b/ext/eigen/eigen.cpp @@ -0,0 +1,675 @@ +#include "rice/Class.hpp" +#include "rice/String.hpp" +#include "rice/Constructor.hpp" +#include "rice/Enum.hpp" + +#include +#include +#include + +using namespace Rice; + +typedef Eigen::Matrix Vector3d; +typedef Eigen::Matrix Matrix4d; +typedef Eigen::Quaternion Quaterniond; +typedef Eigen::Matrix + MatrixXd; +typedef Eigen::Matrix + VectorXd; +typedef Eigen::Transform< double, 3, Eigen::Isometry > Isometry3d; +typedef Eigen::Transform< double, 3, Eigen::Affine > Affine3d; +typedef Eigen::AngleAxis AngleAxisd; + +struct Vector3 +{ + Vector3d* v; + + Vector3(double x, double y, double z) + : v(new Vector3d(x, y, z)) {} + Vector3(Vector3d const& _v) + : v(new Vector3d(_v)) {} + ~Vector3() + { delete v; } + + double x() const { return v->x(); } + double y() const { return v->y(); } + double z() const { return v->z(); } + void setX(double value) { v->x() = value; } + void setY(double value) { v->y() = value; } + void setZ(double value) { v->z() = value; } + + + double norm() const { return v->norm(); } + Vector3* normalize() const { return new Vector3(v->normalized()); } + void normalizeBang() const { v->normalize(); } + + double get(int i) const { return (*v)[i]; } + void set(int i, double value) { (*v)[i] = value; } + + Vector3* operator + (Vector3 const& other) const + { return new Vector3(*v + *other.v); } + Vector3* operator - (Vector3 const& other) const + { return new Vector3(*v - *other.v); } + + Vector3* operator / (double scalar) const + { return new Vector3(*v / scalar); } + + Vector3* negate() const + { return new Vector3(-*v); } + Vector3* scale(double value) const + { return new Vector3(*v * value); } + double dot(Vector3 const& other) const + { return this->v->dot(*other.v); } + Vector3* cross(Vector3 const& other) const + { return new Vector3(this->v->cross(*other.v)); } + bool operator ==(Vector3 const& other) const + { return (*this->v) == (*other.v); } + bool isApprox(Vector3 const& other, double tolerance) + { return v->isApprox(*other.v, tolerance); } +}; + +struct VectorX { + + VectorXd* v; + + VectorX() + : v(new VectorXd()) {} + VectorX(VectorX const& v) + : v(new VectorXd(*v.v)) {} + VectorX(int n) + : v(new VectorXd(n)) {} + VectorX(VectorXd const& _v) + : v(new VectorXd(_v)) {} + ~VectorX() + { delete v; } + + void resize(int n) { v->resize(n); } + void conservativeResize(int n) { v->conservativeResize(n); } + + double norm() const { return v->norm(); } + VectorX* normalize() const { return new VectorX(v->normalized()); } + void normalizeBang() const { v->normalize(); } + + unsigned int size() { return v->size(); } + + double get(int i) const { return (*v)[i]; } + void set(int i, double value) { (*v)[i] = value; } + + VectorX* operator + (VectorX const& other) const + { return new VectorX(*v + *other.v); } + VectorX* operator - (VectorX const& other) const + { return new VectorX(*v - *other.v); } + + VectorX* operator / (double scalar) const + { return new VectorX(*v / scalar); } + + VectorX* negate() const + { return new VectorX(-*v); } + + VectorX* scale(double value) const + { return new VectorX(*v * value); } + + double dot(VectorX const& other) const + { return this->v->dot(*other.v); } + + bool operator ==(VectorX const& other) const + { return (*this->v) == (*other.v); } + + bool isApprox(VectorX const& other, double tolerance) + { return v->isApprox(*other.v, tolerance); } + +}; + +struct Matrix4 +{ + Matrix4d* mx; + + Matrix4() : mx(new Matrix4d()) {} + + Matrix4(Matrix4d const& _mx) + : mx(new Matrix4d(_mx)) {} + + ~Matrix4() + { delete mx; } + + double norm() const { return mx->norm(); } + + int rows() const { return mx->rows(); } + int cols() const { return mx->cols(); } + int size() const { return mx->size(); } + + double get(int i, int j ) const { return (*mx)(i,j); } + void set(int i, int j, double value) { (*mx)(i,j) = value; } + + Matrix4* transpose() const + { return new Matrix4(mx->transpose()); } + + Matrix4* operator + (Matrix4 const& other) const + { return new Matrix4(*mx + *other.mx); } + + Matrix4* operator - (Matrix4 const& other) const + { return new Matrix4(*mx - *other.mx); } + + Matrix4* operator / (double scalar) const + { return new Matrix4(*mx / scalar); } + + Matrix4* negate() const + { return new Matrix4(-*mx); } + + Matrix4* scale(double value) const + { return new Matrix4(*mx * value); } + + Matrix4* dotM (Matrix4 const& other) const + { return new Matrix4(*mx * (*other.mx)); } + + bool operator ==(Matrix4 const& other) const + { return (*this->mx) == (*other.mx); } + + bool isApprox(Matrix4 const& other, double tolerance) + { return mx->isApprox(*other.mx, tolerance); } +}; + +struct JacobiSVD { + typedef Eigen::JacobiSVD EigenT; + EigenT* j; + + JacobiSVD( EigenT const& j ) + : j(new EigenT(j)) {} + JacobiSVD( JacobiSVD const& j ) + : j(new EigenT(*j.j)) {} + ~JacobiSVD() + { delete j; } + + VectorX* solve(VectorX* y) + { return new VectorX(j->solve(*y->v)); } +}; + +struct MatrixX { + + MatrixXd* m; + + MatrixX() : m(new MatrixXd()) {} + MatrixX(const MatrixX& m) : m(new MatrixXd(*m.m)) {} + MatrixX(int rows, int cols) : m(new MatrixXd(rows,cols)) {} + MatrixX(const MatrixXd& _m) : m(new MatrixXd(_m)) {} + ~MatrixX() { delete m; } + + void resize(int rows, int cols) { m->resize(rows,cols); } + void conservativeResize(int rows, int cols) { m->conservativeResize(rows,cols); } + + double norm() const { return m->norm(); } + + unsigned int rows() const { return m->rows(); } + unsigned int cols() const { return m->cols(); } + unsigned int size() const { return m->size(); } + + double get(int i, int j ) const { return (*m)(i,j); } + void set(int i, int j, double value) { (*m)(i,j) = value; } + + VectorX* getRow(int i) const { return new VectorX(m->row(i)); } + void setRow(int i, const VectorX& v) { m->row(i) = *(v.v); } + + VectorX* getColumn(int j) const { return new VectorX(m->col(j)); } + void setColumn(int j, const VectorX& v) { m->col(j) = *(v.v); } + + MatrixX* transpose() const + { return new MatrixX(m->transpose()); } + + MatrixX* operator + (MatrixX const& other) const + { return new MatrixX(*m + *other.m); } + + MatrixX* operator - (MatrixX const& other) const + { return new MatrixX(*m - *other.m); } + + MatrixX* operator / (double scalar) const + { return new MatrixX(*m / scalar); } + + MatrixX* negate() const + { return new MatrixX(-*m); } + + MatrixX* scale (double scalar) const + { return new MatrixX(*m * scalar); } + + VectorX* dotV (VectorX const& other) const + { return new VectorX(*m * *other.v); } + + MatrixX* dotM (MatrixX const& other) const + { return new MatrixX(*m * (*other.m)); } + + JacobiSVD* jacobiSvd(int flags = 0) const + { return new JacobiSVD(m->jacobiSvd(flags)); } + + bool operator ==(MatrixX const& other) const + { return (*this->m) == (*other.m); } + + bool isApprox(MatrixX const& other, double tolerance) + { return m->isApprox(*other.m, tolerance); } +}; + +struct Quaternion +{ + Quaterniond* q; + Quaternion(double w, double x, double y, double z) + : q(new Quaterniond(w, x, y, z)) { } + Quaternion(Quaternion const& q) + : q(new Quaterniond(*q.q)) { } + Quaternion(Quaterniond const& _q) + : q(new Quaterniond(_q)) {} + + ~Quaternion() + { delete q; } + + double w() const { return q->w(); } + double x() const { return q->x(); } + double y() const { return q->y(); } + double z() const { return q->z(); } + void setW(double value) { q->w() = value; } + void setX(double value) { q->x() = value; } + void setY(double value) { q->y() = value; } + void setZ(double value) { q->z() = value; } + + double norm() const { return q->norm(); } + + bool operator ==(Quaternion const& other) const + { return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w(); } + + Quaternion* concatenate(Quaternion const& other) const + { return new Quaternion((*q) * (*other.q)); } + Vector3* transform(Vector3 const& v) const + { return new Vector3((*q) * (*v.v)); } + Quaternion* inverse() const + { return new Quaternion(q->inverse()); } + void normalizeBang() + { q->normalize(); } + Quaternion* normalize() const + { + Quaterniond q = *this->q; + q.normalize(); + return new Quaternion(q); + } + MatrixX* matrix() const + { + return new MatrixX(q->matrix()); + } + + void fromAngleAxis(double angle, Vector3 const& axis) + { + *(this->q) = + Eigen::AngleAxisd(angle, *axis.v); + } + + void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2) + { + *(this->q) = + Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) * + Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) * + Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2)); + } + + void fromMatrix(MatrixX const& matrix) + { + *(this->q) = + Quaterniond(Eigen::Matrix3d(*matrix.m)); + } + + bool isApprox(Quaternion const& other, double tolerance) + { + return q->isApprox(*other.q, tolerance); + } + + Vector3* toEuler() + { + const Eigen::Matrix3d m = q->toRotationMatrix(); + double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); + double y = atan2(-m.coeff(2,0), i); + double x=0,z=0; + if (i > Eigen::NumTraits::dummy_precision()){ + x = ::atan2(m.coeff(1,0), m.coeff(0,0)); + z = ::atan2(m.coeff(2,1), m.coeff(2,2)); + }else{ + z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); + } + return new Vector3(x,y,z); + } +}; + +struct AngleAxis +{ + AngleAxisd* aa; + AngleAxis(double angle, Vector3 const& axis) + : aa(new AngleAxisd(angle, Eigen::Vector3d(*axis.v))){} + AngleAxis(AngleAxis const& aa) + : aa(new AngleAxisd(*aa.aa)) { } + AngleAxis(AngleAxisd const& _aa) + : aa(new AngleAxisd(_aa)) {} + + ~AngleAxis() + { delete aa; } + + bool operator ==(AngleAxis const& other) const + { return angle() == other.angle() && axis() == other.axis(); } + + double angle() const { return aa->angle(); } + Vector3* axis() const { return new Vector3(aa->axis()); } + + AngleAxis* concatenate(AngleAxis const& other) const + { return new AngleAxis(static_cast((*aa) * (*other.aa))); } + + Vector3* transform(Vector3 const& v) const + { return new Vector3((*aa) * (*v.v)); } + + AngleAxis* inverse() const + { return new AngleAxis(aa->inverse()); } + + MatrixX* matrix() const + { + return new MatrixX(aa->matrix()); + } + + void fromQuaternion(Quaternion const& q) + { + *(this->aa) = Eigen::Quaterniond(q.w(), q.x(), q.y(), q.z()); + } + + void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2) + { + *(this->aa) = + Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) * + Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) * + Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2)); + } + + void fromMatrix(MatrixX const& matrix) + { + *(this->aa) = + AngleAxisd(Eigen::Matrix3d(*matrix.m)); + } + + bool isApprox(AngleAxis const& other, double tolerance) + { + return aa->isApprox(*other.aa, tolerance); + } + + Vector3* toEuler() + { + const Eigen::Matrix3d m = aa->toRotationMatrix(); + double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); + double y = atan2(-m.coeff(2,0), i); + double x=0,z=0; + if (i > Eigen::NumTraits::dummy_precision()){ + x = ::atan2(m.coeff(1,0), m.coeff(0,0)); + z = ::atan2(m.coeff(2,1), m.coeff(2,2)); + }else{ + z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); + } + return new Vector3(x,y,z); + } +}; + +#include + +struct Isometry3 +{ + Isometry3d *t; + + Isometry3() : t(new Isometry3d()) { t->setIdentity(); } + Isometry3(const Isometry3& _m) : t(new Isometry3d(*_m.t)) {} + Isometry3(const Isometry3d& _m) : t(new Isometry3d(_m)) {} + ~Isometry3() { delete t; } + + Isometry3* inverse() const + { return new Isometry3( t->inverse() ); } + + Vector3* translation() const + { return new Vector3( t->translation() ); } + + Quaternion* rotation() const + { return new Quaternion( Eigen::Quaterniond(t->linear()) ); } + + Isometry3* concatenate(Isometry3 const& other) const + { return new Isometry3( *t * *other.t ); } + + Vector3* transform(Vector3 const& other) const + { return new Vector3( *t * *other.v ); } + + MatrixX* matrix() const + { return new MatrixX( t->matrix() ); } + + void translate( Vector3 const& other ) const + { t->translate( *other.v ); } + + void pretranslate( Vector3 const& other ) const + { t->pretranslate( *other.v ); } + + void rotate( Quaternion const& other ) const + { t->rotate( *other.q ); } + + void prerotate( Quaternion const& other ) const + { t->prerotate( *other.q ); } + + bool operator ==(Isometry3 const& other) const + { return (*this->t).matrix() == (*other.t).matrix(); } + + bool isApprox(Isometry3 const& other, double tolerance) + { return t->isApprox(*other.t, tolerance); } +}; + +struct Affine3 +{ + Affine3d *t; + + Affine3() : t(new Affine3d()) { t->setIdentity(); } + Affine3(const Affine3& _m) : t(new Affine3d(*_m.t)) {} + Affine3(const Affine3d& _m) : t(new Affine3d(_m)) {} + ~Affine3() { delete t; } + + Affine3* inverse() const + { return new Affine3( t->inverse() ); } + + Vector3* translation() const + { return new Vector3( t->translation() ); } + + Quaternion* rotation() const + { return new Quaternion( Eigen::Quaterniond(t->linear()) ); } + + Affine3* concatenate(Affine3 const& other) const + { return new Affine3( *t * *other.t ); } + + Vector3* transform(Vector3 const& other) const + { return new Vector3( *t * *other.v ); } + + MatrixX* matrix() const + { return new MatrixX( t->matrix() ); } + + void translate( Vector3 const& other ) const + { t->translate( *other.v ); } + + void pretranslate( Vector3 const& other ) const + { t->pretranslate( *other.v ); } + + void rotate( Quaternion const& other ) const + { t->rotate( *other.q ); } + + void prerotate( Quaternion const& other ) const + { t->prerotate( *other.q ); } + + bool operator ==(Affine3 const& other) const + { return (*this->t).matrix() == (*other.t).matrix(); } + + bool isApprox(Affine3 const& other, double tolerance) + { return t->isApprox(*other.t, tolerance); } +}; + + +// The initialization method for this module +extern "C" void Init_eigen() +{ + Rice::Module rb_mEigen = define_module("Eigen"); + + Data_Type rb_Vector3 = define_class_under(rb_mEigen, "Vector3") + .define_constructor(Constructor(), + (Arg("x") = static_cast(0), + Arg("y") = static_cast(0), + Arg("z") = static_cast(0))) + .define_method("__equal__", &Vector3::operator ==) + .define_method("norm", &Vector3::norm) + .define_method("normalize!", &Vector3::normalizeBang) + .define_method("normalize", &Vector3::normalize) + .define_method("[]", &Vector3::get) + .define_method("[]=", &Vector3::set) + .define_method("x", &Vector3::x) + .define_method("y", &Vector3::y) + .define_method("z", &Vector3::z) + .define_method("x=", &Vector3::setX) + .define_method("y=", &Vector3::setY) + .define_method("z=", &Vector3::setZ) + .define_method("+", &Vector3::operator +) + .define_method("-", &Vector3::operator -) + .define_method("/", &Vector3::operator /) + .define_method("-@", &Vector3::negate) + .define_method("*", &Vector3::scale) + .define_method("cross", &Vector3::cross) + .define_method("dot", &Vector3::dot) + .define_method("approx?", &Vector3::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())); + + Data_Type rb_Quaternion = define_class_under(rb_mEigen, "Quaternion") + .define_constructor(Constructor()) + .define_method("__equal__", &Quaternion::operator ==) + .define_method("w", &Quaternion::w) + .define_method("x", &Quaternion::x) + .define_method("y", &Quaternion::y) + .define_method("z", &Quaternion::z) + .define_method("w=", &Quaternion::setW) + .define_method("x=", &Quaternion::setX) + .define_method("y=", &Quaternion::setY) + .define_method("z=", &Quaternion::setZ) + .define_method("norm", &Quaternion::norm) + .define_method("concatenate", &Quaternion::concatenate) + .define_method("inverse", &Quaternion::inverse) + .define_method("transform", &Quaternion::transform) + .define_method("matrix", &Quaternion::matrix) + .define_method("normalize!", &Quaternion::normalizeBang) + .define_method("normalize", &Quaternion::normalize) + .define_method("approx?", &Quaternion::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())) + .define_method("to_euler", &Quaternion::toEuler) + .define_method("from_euler", &Quaternion::fromEuler) + .define_method("from_angle_axis", &Quaternion::fromAngleAxis) + .define_method("from_matrix", &Quaternion::fromMatrix); + + Data_Type rb_AngleAxis = define_class_under(rb_mEigen, "AngleAxis") + .define_constructor(Constructor()) + .define_method("__equal__", &AngleAxis::operator ==) + .define_method("angle", &AngleAxis::angle) + .define_method("axis", &AngleAxis::axis) + .define_method("concatenate", &AngleAxis::concatenate) + .define_method("inverse", &AngleAxis::inverse) + .define_method("transform", &AngleAxis::transform) + .define_method("matrix", &AngleAxis::matrix) + .define_method("approx?", &AngleAxis::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())) + .define_method("to_euler", &AngleAxis::toEuler) + .define_method("from_euler", &AngleAxis::fromEuler) + .define_method("from_quaternion", &AngleAxis::fromQuaternion) + .define_method("from_matrix", &AngleAxis::fromMatrix); + + Data_Type rb_VectorX = define_class_under(rb_mEigen, "VectorX") + .define_constructor(Constructor(), + (Arg("rows") = static_cast(0))) + .define_method("resize", &VectorX::resize) + .define_method("__equal__", &VectorX::operator ==) + .define_method("norm", &VectorX::norm) + .define_method("normalize!", &VectorX::normalizeBang) + .define_method("normalize", &VectorX::normalize) + .define_method("size", &VectorX::size) + .define_method("[]", &VectorX::get) + .define_method("[]=", &VectorX::set) + .define_method("+", &VectorX::operator +) + .define_method("-", &VectorX::operator -) + .define_method("/", &VectorX::operator /) + .define_method("-@", &VectorX::negate) + .define_method("*", &VectorX::scale) + .define_method("dot", &VectorX::dot) + .define_method("approx?", &VectorX::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())); + + Data_Type rb_Matrix4 = define_class_under(rb_mEigen, "Matrix4") + .define_constructor(Constructor()) + .define_method("__equal__", &Matrix4::operator ==) + .define_method("T", &Matrix4::transpose) + .define_method("norm", &Matrix4::norm) + .define_method("rows", &Matrix4::rows) + .define_method("cols", &Matrix4::cols) + .define_method("size", &Matrix4::size) + .define_method("[]", &Matrix4::get) + .define_method("[]=", &Matrix4::set) + .define_method("+", &Matrix4::operator +) + .define_method("-", &Matrix4::operator -) + .define_method("/", &Matrix4::operator /) + .define_method("-@", &Matrix4::negate) + .define_method("*", &Matrix4::scale) + .define_method("dotM", &Matrix4::dotM) + .define_method("approx?", &Matrix4::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())); + + rb_mEigen.const_set("ComputeFullU", INT2FIX(Eigen::ComputeFullU)); + rb_mEigen.const_set("ComputeThinU", INT2FIX(Eigen::ComputeThinU)); + rb_mEigen.const_set("ComputeThinV", INT2FIX(Eigen::ComputeThinV)); + + Data_Type rb_JacobiSVD = define_class_under(rb_mEigen, "JacobiSVD") + .define_method("solve", &JacobiSVD::solve); + + Data_Type rb_MatrixX = define_class_under(rb_mEigen, "MatrixX") + .define_constructor(Constructor(), + (Arg("rows") = static_cast(0), + Arg("cols") = static_cast(0))) + .define_method("resize", &MatrixX::resize) + .define_method("__equal__", &MatrixX::operator ==) + .define_method("T", &MatrixX::transpose) + .define_method("norm", &MatrixX::norm) + .define_method("rows", &MatrixX::rows) + .define_method("cols", &MatrixX::cols) + .define_method("size", &MatrixX::size) + .define_method("[]", &MatrixX::get) + .define_method("[]=", &MatrixX::set) + .define_method("row", &MatrixX::getRow) + .define_method("setRow", &MatrixX::setRow) + .define_method("col", &MatrixX::getColumn) + .define_method("setCol", &MatrixX::setColumn) + .define_method("+", &MatrixX::operator +) + .define_method("-", &MatrixX::operator -) + .define_method("/", &MatrixX::operator /) + .define_method("-@", &MatrixX::negate) + .define_method("*", &MatrixX::scale) + .define_method("dotV", &MatrixX::dotV) + .define_method("dotM", &MatrixX::dotM) + .define_method("jacobiSvd", &MatrixX::jacobiSvd, (Arg("flags") = 0)) + .define_method("approx?", &MatrixX::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())); + + Data_Type rb_Isometry3 = define_class_under(rb_mEigen, "Isometry3") + .define_constructor(Constructor()) + .define_method("__equal__", &Isometry3::operator ==) + .define_method("approx?", &Isometry3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())) + .define_method("inverse", &Isometry3::inverse) + .define_method("translation", &Isometry3::translation) + .define_method("rotation", &Isometry3::rotation) + .define_method("concatenate", &Isometry3::concatenate) + .define_method("transform", &Isometry3::transform) + .define_method("matrix", &Isometry3::matrix) + .define_method("translate", &Isometry3::translate) + .define_method("pretranslate", &Isometry3::pretranslate) + .define_method("rotate", &Isometry3::rotate) + .define_method("prerotate", &Isometry3::prerotate); + + Data_Type rb_Affine3 = define_class_under(rb_mEigen, "Affine3") + .define_constructor(Constructor()) + .define_method("__equal__", &Affine3::operator ==) + .define_method("approx?", &Affine3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits::dummy_precision())) + .define_method("inverse", &Affine3::inverse) + .define_method("translation", &Affine3::translation) + .define_method("rotation", &Affine3::rotation) + .define_method("concatenate", &Affine3::concatenate) + .define_method("transform", &Affine3::transform) + .define_method("matrix", &Affine3::matrix) + .define_method("translate", &Affine3::translate) + .define_method("pretranslate", &Affine3::pretranslate) + .define_method("rotate", &Affine3::rotate) + .define_method("prerotate", &Affine3::prerotate); +} + diff --git a/ext/eigen/extconf.rb b/ext/eigen/extconf.rb new file mode 100644 index 0000000..777b488 --- /dev/null +++ b/ext/eigen/extconf.rb @@ -0,0 +1,10 @@ +require "mkmf-rice" + +RbConfig::CONFIG['CXXFLAGS'].gsub! "-Wdeclaration-after-statement", '' +RbConfig::CONFIG['CXXFLAGS'].gsub! "-Wimplicit-function-declaration", '' + +if !pkg_config('eigen3') + raise "cannot find the eigen3 pkg-config package in #{ENV['PKG_CONFIG_PATH']}" +end + +create_makefile("eigen/eigen") diff --git a/lib/eigen.rb b/lib/eigen.rb new file mode 100644 index 0000000..b614231 --- /dev/null +++ b/lib/eigen.rb @@ -0,0 +1,15 @@ +# Ruby bindings for the Eigen C++ linear algebra library +module Eigen +end + +require 'eigen/eigen' + +require 'eigen/affine3' +require 'eigen/angle_axis' +require 'eigen/isometry3' +require 'eigen/matrix4' +require 'eigen/matrixx' +require 'eigen/quaternion' +require 'eigen/vector3' +require 'eigen/vectorx' +require 'eigen/version' diff --git a/lib/eigen/affine3.rb b/lib/eigen/affine3.rb new file mode 100644 index 0000000..b68cebf --- /dev/null +++ b/lib/eigen/affine3.rb @@ -0,0 +1,35 @@ +module Eigen + class Affine3 + def self.Identity + Affine3.new + end + + def self.from_position_orientation( v, q ) + i = Affine3.Identity + i.prerotate( q ) + i.pretranslate( v ) + i + end + + def dup + raise NotImplementedError + end + + def ==(q) + q.kind_of?(self.class) && + __equal__(q) + end + + def *(obj) + if obj.kind_of?(Affine3) + concatenate(obj) + else + transform(obj) + end + end + + def to_s + matrix.to_s + end + end +end diff --git a/lib/eigen/angle_axis.rb b/lib/eigen/angle_axis.rb new file mode 100644 index 0000000..01c1205 --- /dev/null +++ b/lib/eigen/angle_axis.rb @@ -0,0 +1,90 @@ +module Eigen + # Representation and manipulation of an angle axis + class AngleAxis + def dup + AngleAxis.new(angle, axis) + end + + # Returns the angle axis as [angle, x, y, z] + def to_a; [angle, axis.to_a] end + + def from_a (array) + aa = AngleAxis.new(array[0], Eigen::Vector3.new(array[1][0], array[1][1], array[1][2])) + aa + end + + # Returns the identity unit quaternion (identity rotation) + def self.Identity + AngleAxis.new(0, Eigen::Vector3.new(1, 0, 0)) + end + + # Creates a angle axis from a quaternion + def self.from_quaternion(*args) + aa = new(0, Eigen::Vector3.new(1, 0, 0)) + aa.from_quaternion(*args) + aa + end + + # Creates a quaternion from a set of euler angles. + # + # See Quaternion#from_euler for details + def self.from_euler(*args) + aa = new(0, Eigen::Vector3.new(1, 0, 0)) + aa.from_euler(*args) + aa + end + + # Creates a quaternion from a rotation matrix + def self.from_matrix(m) + aa = new(0, Eigen::Vector3.new(1, 0, 0)) + aa.from_matrix(m) + aa + end + + # Returns a scaled axis representation that is equivalent to this + # quaternion + # + # @param [Float] eps see {#to_angle_axis} + # @return [Vector3] + def to_scaled_axis(eps = 1e-12) + return axis * angle + end + + # Concatenates with another angle axis or transforms a vector + def *(obj) + if obj.kind_of?(AngleAxis) + concatenate(obj) + else + transform(obj) + end + end + + def _dump(level) # :nodoc: + Marshal.dump(to_a) + end + + def self._load(coordinates) # :nodoc: + aa = new(0, Eigen::Vector3.new(1, 0, 0)) + aa.from_a(Marshal.load(coordinates)) + aa + end + + def to_s # :nodoc: + "AngleAxis( angle #{angle}, axis(#{axis.x}, #{axis.y}, #{axis.z}))" + end + + # Tests for equality + # + # Since Quaternion stores the coordinates as floating-point values, this is + # a bad test. Use + # + # (v - other_v).norm < threshold + # + # instead + def ==(q) + q.kind_of?(self.class) && + __equal__(q) + end + end +end + diff --git a/lib/eigen/isometry3.rb b/lib/eigen/isometry3.rb new file mode 100644 index 0000000..865d62c --- /dev/null +++ b/lib/eigen/isometry3.rb @@ -0,0 +1,36 @@ +module Eigen + class Isometry3 + def self.Identity + Isometry3.new + end + + def self.from_position_orientation( v, q ) + i = Isometry3.Identity + i.prerotate( q ) + i.pretranslate( v ) + i + end + + def dup + raise NotImplementedError + end + + def ==(q) + q.kind_of?(self.class) && + __equal__(q) + end + + def *(obj) + if obj.kind_of?(Isometry3) + concatenate(obj) + else + transform(obj) + end + end + + def to_s + matrix.to_s + end + end +end + diff --git a/lib/eigen/matrix4.rb b/lib/eigen/matrix4.rb new file mode 100644 index 0000000..474f477 --- /dev/null +++ b/lib/eigen/matrix4.rb @@ -0,0 +1,73 @@ +module Eigen + # Matrix 4x4 + class Matrix4 + def self.from_a(*args) + m = new + m.from_a(*args) + m + end + + # Returns the array value in a vector + def to_a(column_major=true) + a = [] + if column_major + for j in 0..3 + for i in 0..3 + a << self[i,j] + end + end + else + for i in 0..3 + for j in 0..3 + a << self[i,j] + end + end + end + a + end + + # sets matrix from a 1d array + def from_a(array, column_major=true) + array.each_index do |i| + v = array[i] + if !v + v = 0.0 + end + if column_major + self[i%4,i/4] = v + else + self[i/4,i%4] = v + end + end + end + + def ==(m) + m.kind_of?(self.class) && + __equal__(m) + end + + def to_s # :nodoc: + str = "Matrix4(\n" + for i in 0..3 + for j in 0..3 + str += "#{self[i,j]} " + end + str[-1] = "\n" + end + str += ")" + str + end + + def _dump(level) # :nodoc: + Marshal.dump({'rows' => rows, 'cols' => cols, 'data' => to_a}) + end + + def self._load(coordinates) # :nodoc: + o = Marshal.load(coordinates) + m = new() + m.from_a(o['data']) + m + end + end +end + diff --git a/lib/eigen/matrixx.rb b/lib/eigen/matrixx.rb new file mode 100644 index 0000000..3534248 --- /dev/null +++ b/lib/eigen/matrixx.rb @@ -0,0 +1,95 @@ +module Eigen + # Abritary size vector + class MatrixX + def dup + MatrixX.from_a(to_a, rows, cols) + end + + def self.from_a(*args) + m = new + m.from_a(*args) + m + end + + # Returns the array value in a vector + def to_a(column_major=true) + a = [] + if column_major + for j in 0..cols()-1 + for i in 0..rows()-1 + a << self[i,j] + end + end + else + for i in 0..rows()-1 + for j in 0..cols()-1 + a << self[i,j] + end + end + end + a + end + + # sets matrix from a 1d array + def from_a(array,nrows=-1,ncols=-1,column_major=true) + if nrows == -1 && ncols == -1 + nrows = rows + ncols = cols + elsif nrows == -1 + nrows = array.size / ncols + elsif ncols == -1 + ncols = array.size / nrows + end + resize(nrows,ncols) + array.each_index do |i| + v = array[i] + if !v + v = 0.0 + end + if column_major + self[i%nrows,i/nrows] = v + else + self[i/ncols,i%ncols] = v + end + end + end + + def pretty_print(pp) + for i in 0..rows()-1 + for j in 0..cols()-1 + pp.text " #{self[i,j]}" + end + pp.text "\n" + end + end + + def ==(m) + m.kind_of?(self.class) && + __equal__(m) + end + + def to_s # :nodoc: + str = "MatrixX(\n" + for i in 0..rows()-1 + for j in 0..cols()-1 + str += "#{self[i,j]} " + end + str[-1] = "\n" + end + str += ")" + str + end + + def _dump(level) # :nodoc: + Marshal.dump({'rows' => rows, 'cols' => cols, 'data' => to_a}) + end + + def self._load(coordinates) # :nodoc: + o = Marshal.load(coordinates) + m = new(o['rows'],o['cols']) + m.from_a(o['data'],o['rows'],o['cols']) + m + end + end +end + diff --git a/lib/eigen/quaternion.rb b/lib/eigen/quaternion.rb new file mode 100644 index 0000000..e1395ca --- /dev/null +++ b/lib/eigen/quaternion.rb @@ -0,0 +1,253 @@ +module Eigen + # Representation and manipulation of a quaternion + class Quaternion + def dup + Quaternion.new(w, x, y, z) + end + + # Returns the quaternion as [w, x, y, z] + def to_a; [w, x, y, z] end + + # Returns the identity unit quaternion (identity rotation) + def self.Identity + Quaternion.new(1, 0, 0, 0) + end + + # DEPRECATED: please use identity instead. Returns the unit quaternion (identity rotation) + def self.Unit + warn "[DEPRECATED] Quaternion.unit, please use Quaternion.identity." + self.Identity + end + + # Creates a quaternion from an angle and axis description + def self.from_angle_axis(*args) + q = new(1, 0, 0, 0) + q.from_angle_axis(*args) + q + end + + # Returns an angle,axis representation equivalent to this quaternion + # + # If the angle turns out to be PI, there are two solutions and the one + # with positive Z component is chosen. + # + # @param [Float] eps if the angle turns out to be closer to zero than eps, the + # rotation axis is undefined and chosen to be the Z axis. + # + # @return [(Float,Vector3)] the angle and axis. The angle is in [0, PI] + def to_angle_axis(eps = 1e-12) + w, x, y, z = to_a + norm = Math.sqrt(x*x+y*y+z*z); + if norm < eps + return 0, Eigen::Vector3.new(0,0,1); + end + + angle = 2.0 * Math.atan2(norm, w); + axis = Eigen::Vector3.new(x, y, z) / norm + return angle, axis + end + + # Returns a scaled axis representation that is equivalent to this + # quaternion + # + # @param [Float] eps see {#to_angle_axis} + # @return [Vector3] + def to_scaled_axis(eps = 1e-12) + angle, axis = to_angle_axis(eps) + return axis * angle + end + + # Creates a quaternion from a set of euler angles. + # + # See Quaternion#from_euler for details + def self.from_euler(*args) + q = new(1, 0, 0, 0) + q.from_euler(*args) + q + end + + # Creates a quaternion from a rotation matrix + def self.from_matrix(m) + q = new(1, 0, 0, 0) + q.from_matrix(m) + q + end + + # Extracts the yaw angle from this quaternion + # + # It decomposes the quaternion in euler angles using to_euler + # and returns the first element. See #to_euler for details. + def yaw + to_euler[0] + end + + def pitch + to_euler[1] + end + + def roll + to_euler[2] + end + + # The inverse of #yaw + def self.from_yaw(yaw) + from_euler(Eigen::Vector3.new(yaw, 0, 0), 2, 1, 0) + end + + # Concatenates with another quaternion or transforms a vector + def *(obj) + if obj.kind_of?(Quaternion) + concatenate(obj) + else + transform(obj) + end + end + + def _dump(level) # :nodoc: + Marshal.dump(to_a) + end + + def self._load(coordinates) # :nodoc: + new(*Marshal.load(coordinates)) + end + + def to_s # :nodoc: + "Quaternion(#{w}, (#{x}, #{y}, #{z}))" + end + + # Tests for equality + # + # Since Quaternion stores the coordinates as floating-point values, this is + # a bad test. Use + # + # (v - other_v).norm < threshold + # + # instead + def ==(q) + q.kind_of?(self.class) && + __equal__(q) + end + + def re + w + end + + def re=(value) + self.w = value + end + + def im + [x,y,z] + end + + def im=(value) + self.x, self.y, self.z = *value + end + + ## + # :method: w + + ## + # :method: x + + ## + # :method: y + + ## + # :method: z + + ## + # :method: w= + + ## + # :method: x= + + ## + # :method: y= + + ## + # :method: z= + + ## + # :method: concatenate + # :call-seq: + # concatenate(q) + # + # Returns the rotation in which +q+ is applied first and +self+ second + + ## + # :method: transform + # :call-seq: + # transform(v) + # + # Transforms the given Eigen::Vector3 by the rotation represented with + # this quaternion + + ## + # :method: normalize! + # + # Normalizes this quaternion + + ## + # :method: normalize + # + # Returns a quaternion that is a normalized version of +self+ + + ## + # :method: approx? + # :call-seq: + # approx?(q, tolerance) + # + # Returns true if +self+ and +q+ do not differ from more than + # +tolerance+. The comparison is done on a coordinate basis. + + ## + # :method: to_euler + # :call-seq: + # to_euler => Eigen::Vector3(a0, a1, a2) + # + # Decomposes this quaternion in euler angles so that +self+ can be + # obtained by applying the following rotations in order: + # + # rotation of a2 around x-axis + # rotation of a1 around y-axis + # rotation of a0 around z-axis + # + # assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2) + # + # note that + # + # self == Quaternion.from_euler(to_euler, axis0, axis1, axis2) + + ## + # :method: from_euler + # :call-seq: + # from_euler(Eigen::Vector3(a0, a1, a2), axis0, axis1, axis2) + # + # Resets this quaternion so that it represents the rotation obtained by + # applying the following rotations in order: + # + # rotation of a2 around axis2 + # rotation of a1 around axis1 + # rotation of a0 around axis0 + # + # note that + # + # self == Quaternion.from_euler(to_euler, axis0, axis1, axis2) + + ## + # :method: inverse + # :call-seq: + # inverse => quaternion + # + # Computes the quaternion that is inverse of this one + + + # @return [Qt::Quaternion] the Qt quaternion that is identical to this + # one + def to_qt + Qt::Quaternion.new(w, x, y, z) + end + end +end + diff --git a/lib/eigen/vector3.rb b/lib/eigen/vector3.rb new file mode 100644 index 0000000..4288e71 --- /dev/null +++ b/lib/eigen/vector3.rb @@ -0,0 +1,178 @@ +module Eigen + # 3-dimensional vector + class Vector3 + # Returns a vector with all values set to Base.unset + def self.Unset + return Vector3.new(Base.unset, Base.unset, Base.unset) + end + + def dup + Vector3.new(x, y, z) + end + + # Returns the [x, y, z] tuple + def to_a; [x, y, z] end + + # Returns the (1, 0, 0) unit vector + def self.UnitX() + return Vector3.new(1, 0, 0) + end + + # Returns the (0, 1, 0) unit vector + def self.UnitY() + return Vector3.new(0, 1, 0) + end + + # Returns the (0, 0, 1) unit vector + def self.UnitZ() + return Vector3.new(0, 0, 1) + end + + # returns the (0, 0, 0) vector + def self.Zero() + return Vector3.new(0, 0, 0) + end + + # Returns the angle formed by +self+ and +v+, oriented from +self+ to + # +v+ + def angle_to(v) + ret = Math.atan2(v.y, v.x) - Math.atan2(y, x) + if ret > Math::PI + ret -= 2*Math::PI + end + if ret < -Math::PI + ret += 2*Math::PI + end + ret + end + + # Tests for equality + # + # Since Vector3 stores the coordinates as floating-point values, this is + # a bad test. Use + # + # q.approx?(other_q, tolerance) + # + # instead + def ==(v) + v.kind_of?(self.class) && + __equal__(v) + end + + # Support for Marshal + def _dump(level) # :nodoc: + Marshal.dump(to_a) + end + + # Support for Marshal + def self._load(coordinates) # :nodoc: + new(*Marshal.load(coordinates)) + end + + def to_s # :nodoc: + "Vector3(#{x}, #{y}, #{z})" + end + + def data + [x, y, z] + end + + def data=(value) + self.x,self.y,self.z = value + end + + ## + # :method: == + + ## + # :method: [] + # + # Returns the i-th coordinate + + ## + # :method: []= + # + # Sets the i-th coordinate + + ## + # :method: x + + ## + # :method: y + + ## + # :method: z + + ## + # :method: x= + + ## + # :method: y= + + ## + # :method: z= + + ## + # :method: + + + ## + # :method: - + + ## + # :method: -@ + + ## + # :method: * + # :call-seq: + # a * scalar => b + # + # Returns +a+ scaled with the given scalar + + ## + # :method: cross + # :call-seq: + # cross(b) => c + # + # Returns the cross product of +self+ with +b+ + + ## + # :method: norm + # + # Returns the norm of +self+ + + ## + # :method: normalize! + # + # Makes this vector unit-length + + ## + # :method: normalize + # + # Returns a vector that has the same direction than +self+ but unit + # length + + ## + # Computes the signed angle between two vectors, using the provided + # vector as "positive" rotation direction + # + # The returned angle A is so that the rotation defined by A and axis + # will transform +self+ into +v+ + def signed_angle_to(v, axis) + dot_p = self.dot(v) + dir = self.cross(v).dot(axis) + + unsigned = Math.acos(dot_p / norm / v.norm) + if dir > 0 + return unsigned + else + return -unsigned + end + end + + # @return [Qt::Quaternion] the Qt vector that is identical to this + # one + def to_qt + Qt::Vector3D.new(x, y, z) + end + end +end diff --git a/lib/eigen/vectorx.rb b/lib/eigen/vectorx.rb new file mode 100644 index 0000000..fec80e3 --- /dev/null +++ b/lib/eigen/vectorx.rb @@ -0,0 +1,54 @@ +module Eigen + # Abritary size vector + class VectorX + def dup + VectorX.from_a(to_a) + end + + # Returns the array value in a vector + def to_a() + a = [] + for i in 0..size()-1 + a << self[i] + end + a + end + + def self.from_a(array) + v = VectorX.new + v.from_a(array) + v + end + + def from_a(array) + resize(array.size()) + for i in 0..array.size()-1 + self[i] = array[i] + end + end + + def ==(v) + v.kind_of?(self.class) && + __equal__(v) + end + + def to_s # :nodoc: + str = "VectorX(" + for i in 0..size()-1 + str += "#{self[i]} " + end + str[-1] = ")" + str + end + + def _dump(level) # :nodoc: + Marshal.dump(to_a) + end + + def self._load(coordinates) # :nodoc: + m = new() + m.from_a(Marshal.load(coordinates)) + m + end + end +end diff --git a/lib/eigen/version.rb b/lib/eigen/version.rb new file mode 100644 index 0000000..a5e5f32 --- /dev/null +++ b/lib/eigen/version.rb @@ -0,0 +1,3 @@ +module Eigen + VERSION = "0.1.0" +end diff --git a/test/affine3_test.rb b/test/affine3_test.rb new file mode 100644 index 0000000..0b7717a --- /dev/null +++ b/test/affine3_test.rb @@ -0,0 +1,79 @@ +require 'test_helper' + +class TC_Eigen_Affine3 < Minitest::Test + def test_base + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t = Eigen::Affine3.from_position_orientation( v, q ) + + vt = t.translation + qt = t.rotation + assert_equal( v, vt ) + assert_equal( q, qt ) + end + + def test_rotation + v = Eigen::Vector3.new(0, 0, 0) + q = Eigen::Quaternion.from_angle_axis( 1.2, Eigen::Vector3.new( 0.1, 0.2, 0.3 ).normalize() ) + t = Eigen::Affine3.from_position_orientation( v, q ) + + p1 = Eigen::Vector3.new(1,1,1) + assert( (t * p1).approx?(q * p1) ) + assert( (t.rotation * p1).approx?(q * p1) ) + + vt = t.translation + qt = t.rotation + assert( v.approx?( vt ) ) + assert( q.approx?( qt ) ) + end + + def test_composition + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(0, 0, 1, 0) + t = Eigen::Affine3.from_position_orientation( v, q ) + + v2 = Eigen::Vector3.new(1, 2, 3) + q2 = Eigen::Quaternion.new(0, 0, 1, 0) + t2 = Eigen::Affine3.from_position_orientation( v2, q2 ) + + # transform a vector + p = Eigen::Vector3.new( 0, 0, 0 ) + r = t * p + + assert_equal( Eigen::Vector3.new( 1, 2, 3 ), r ) + + # perform composite + c = t2 * t + assert_equal( + Eigen::Affine3.from_position_orientation( + Eigen::Vector3.new( 0, 4, 0 ), Eigen::Quaternion.Identity ), + c ) + end + + def test_inverse + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(0, 0, 1, 0) + t = Eigen::Affine3.from_position_orientation( v, q ) + + i = t.inverse() * t + + assert_equal( i, Eigen::Affine3.Identity ) + end + + def test_approx_p_returns_true_on_equality + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t = Eigen::Affine3.from_position_orientation( v, q ) + assert t.approx?(t) + end + + def test_approx_p_returns_true_on_inequality + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t1 = Eigen::Affine3.from_position_orientation( v, q ) + v = Eigen::Vector3.new(2, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t2 = Eigen::Affine3.from_position_orientation( v, q ) + refute t1.approx?(t2) + end +end diff --git a/test/angleaxis_test.rb b/test/angleaxis_test.rb new file mode 100644 index 0000000..8430b1a --- /dev/null +++ b/test/angleaxis_test.rb @@ -0,0 +1,76 @@ +require 'test_helper' + +class TC_Eigen_AngleAxis < Minitest::Test + def test_base + aa = Eigen::AngleAxis.new(0, Eigen::Vector3.new(1, 0, 0)) + assert_equal(0, aa.angle) + assert_equal(1, aa.axis.x) + assert_equal(0, aa.axis.y) + assert_equal(0, aa.axis.z) + end + + def test_to_euler + aa = Eigen::AngleAxis.new(1, Eigen::Vector3.new(1, 0, 0)) + result = aa.to_euler + assert_equal([0, 0, 1], result.to_a) + end + + def test_to_euler_to_quaternion + aa = Eigen::AngleAxis.new(0.5, Eigen::Vector3.new(1, 0, 0)) + + v = aa.to_euler + result = Eigen::AngleAxis.from_euler(v, 2, 1, 0) + + assert(aa.approx?(result, 0.0001)) + end + + def test_approx_returns_true_on_equality + aa = Eigen::AngleAxis.new(0, Eigen::Vector3.new(1, 0, 0)) + assert aa.approx?(aa) + end + + def test_approx_returns_true_on_angleaxis_that_are_different_by_epsilon + aa1 = Eigen::AngleAxis.new(0, Eigen::Vector3.new(1, 0, 0)) + aa2 = Eigen::AngleAxis.new(0, Eigen::Vector3.new(1 + Float::EPSILON, 0 + Float::EPSILON, 0 + Float::EPSILON)) + assert aa1.approx?(aa2) + end + + def test_approx_returns_false_on_angleaxis_that_are_different + q1 = Eigen::AngleAxis.new(1, Eigen::Vector3.new(1, 1, 1)) + q2 = Eigen::AngleAxis.new(2, Eigen::Vector3.new(2, 2, 2)) + refute q1.approx?(q2) + end + + def test_approx_returns_true_on_angleaxis_that_are_less_different_than_the_proqided_accuracy + q1 = Eigen::AngleAxis.new(1, Eigen::Vector3.new(1, 1, 1)) + q2 = Eigen::AngleAxis.new(1.5, Eigen::Vector3.new(1.5, 1.5, 1.5)) + assert q1.approx?(q2, 2) + end + + def test_quaternion_ + aa = Eigen::AngleAxis.from_quaternion( Eigen::Quaternion.new(0, 1, 0, 0)) + v = Eigen::Vector3.new(0, 1, 0) + + assert( Eigen::Vector3.new(0, -1, 0).approx?( aa*v, 0.0001 ) ) + end + + def test_inverse + aa1 = Eigen::AngleAxis.from_euler( Eigen::Vector3.new(1, 0, 0), 2, 1, 0) + aa2 = Eigen::AngleAxis.from_euler( Eigen::Vector3.new(-1, 0, 0), 2, 1, 0) + + assert(aa1.to_euler.approx?( aa2.inverse.to_euler, 0.0001 )) + end + + def test_dump_load + aa = Eigen::AngleAxis.new(0, Eigen::Vector3.new(1, 0, 0)) + dumped = Marshal.dump(aa) + loaded = Marshal.load(dumped) + assert(aa.approx?(loaded, 0.0001)) + end + + def test_dup + aa = Eigen::AngleAxis.new(0, Eigen::Vector3.new(0, 0, 0)) + assert(aa.dup.approx?(aa, 0.0001)) + end +end + diff --git a/test/isometry3_test.rb b/test/isometry3_test.rb new file mode 100644 index 0000000..6b9a4b7 --- /dev/null +++ b/test/isometry3_test.rb @@ -0,0 +1,79 @@ +require 'test_helper' + +class TC_Eigen_Isometry3 < Minitest::Test + def test_base + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t = Eigen::Isometry3.from_position_orientation( v, q ) + + vt = t.translation + qt = t.rotation + assert_equal( v, vt ) + assert_equal( q, qt ) + end + + def test_rotation + v = Eigen::Vector3.new(0, 0, 0) + q = Eigen::Quaternion.from_angle_axis( 1.2, Eigen::Vector3.new( 0.1, 0.2, 0.3 ).normalize() ) + t = Eigen::Isometry3.from_position_orientation( v, q ) + + p1 = Eigen::Vector3.new(1,1,1) + assert( (t * p1).approx?(q * p1) ) + assert( (t.rotation * p1).approx?(q * p1) ) + + vt = t.translation + qt = t.rotation + assert( v.approx?( vt ) ) + assert( q.approx?( qt ) ) + end + + def test_composition + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(0, 0, 1, 0) + t = Eigen::Isometry3.from_position_orientation( v, q ) + + v2 = Eigen::Vector3.new(1, 2, 3) + q2 = Eigen::Quaternion.new(0, 0, 1, 0) + t2 = Eigen::Isometry3.from_position_orientation( v2, q2 ) + + # transform a vector + p = Eigen::Vector3.new( 0, 0, 0 ) + r = t * p + + assert_equal( Eigen::Vector3.new( 1, 2, 3 ), r ) + + # perform composite + c = t2 * t + assert_equal( + Eigen::Isometry3.from_position_orientation( + Eigen::Vector3.new( 0, 4, 0 ), Eigen::Quaternion.Identity ), + c ) + end + + def test_inverse + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(0, 0, 1, 0) + t = Eigen::Isometry3.from_position_orientation( v, q ) + + i = t.inverse() * t + + assert_equal( i, Eigen::Isometry3.Identity ) + end + + def test_approx_p_returns_true_on_equality + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t = Eigen::Isometry3.from_position_orientation( v, q ) + assert t.approx?(t) + end + + def test_approx_p_returns_true_on_inequality + v = Eigen::Vector3.new(1, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t1 = Eigen::Isometry3.from_position_orientation( v, q ) + v = Eigen::Vector3.new(2, 2, 3) + q = Eigen::Quaternion.new(1, 0, 0, 0) + t2 = Eigen::Isometry3.from_position_orientation( v, q ) + refute t1.approx?(t2) + end +end diff --git a/test/matrixx_test.rb b/test/matrixx_test.rb new file mode 100644 index 0000000..2873777 --- /dev/null +++ b/test/matrixx_test.rb @@ -0,0 +1,125 @@ +require 'test_helper' + +class TC_Eigen_MatrixX < Minitest::Test + def test_base + m = Eigen::MatrixX.new(2,3) + assert_equal(m.rows, 2) + assert_equal(m.cols, 3) + assert_equal(m.size, 6) + end + + def test_base_vector + v = Eigen::VectorX.new(10) + assert_equal(v.size, 10) + end + + def test_set + m = Eigen::MatrixX.new(2,2) + m[0,0]=2.0 + m[1,0]=1.0 + m[0,1]=-1.0 + m[1,1]=-2.0 + assert_equal(m.to_a,[2.0,1.0,-1.0,-2.0]) + m2 = Eigen::MatrixX.new(2,2) + m2.from_a(m.to_a,2,2) + assert_equal(m, m2) + end + + def test_set_from_a + m = Eigen::MatrixX.new(2,3) + m.from_a([0,1,2,3,4,5],2,3) + assert_equal(m[0,0],0.0) + assert_equal(m[1,0],1.0) + assert_equal(m[0,1],2.0) + assert_equal(m[1,1],3.0) + assert_equal(m[0,2],4.0) + assert_equal(m[1,2],5.0) + end + + def test_set_row + m = Eigen::MatrixX.new(3,4) + v = Eigen::VectorX.new(4) + v.from_a([-3.1,-4.2,5.3,6.4]) + m.setRow(0,v) + m.setRow(2,v) + v2 = Eigen::VectorX.new(4) + v2.from_a([1.0,2.0,3.0,4.0]) + m.setRow(1, v2) + assert_equal(m.row(0).to_a, v.to_a) + assert_equal(m.row(2).to_a, v.to_a) + assert_equal(m.row(1).to_a, v2.to_a) + end + + def test_set_col_add + m = Eigen::MatrixX.new(2,2) + v = Eigen::VectorX.new(2) + v.from_a([1.0,0.0]) + v2 = Eigen::VectorX.new(2) + v2.from_a([10.0,20.0]) + m.setCol(0,v) + m.setCol(1,v2) + assert_equal(m.to_a, [1.0, 0.0, 10.0, 20.0]) + v3 = Eigen::VectorX.new(2) + v3[0] = 11.0 + v3[1] = 20.0 + assert_equal(v3,v+v2) + end + + def test_from_a_col_row_major + m = Eigen::MatrixX.new(2,2) + a = [0,1,2,3] + + m.from_a(a,2,2,true) + assert_equal(m[0,0],0) + assert_equal(m[1,0],1) + assert_equal(m[0,1],2) + assert_equal(m[1,1],3) + + m.from_a(a,2,2,false) + assert_equal(m[0,0],0) + assert_equal(m[0,1],1) + assert_equal(m[1,0],2) + assert_equal(m[1,1],3) + end + + def test_matrix_dump_load + m = Eigen::MatrixX.new(9,7) + l = 9*7 + for i in 0..l-1 + m[i%9,i/9] = i + end + dumped = Marshal.dump(m) + loaded = Marshal.load(dumped) + assert m.approx?(loaded) + end + + def test_dup + m = Eigen::MatrixX.new(9,7) + l = 9*7 + for i in 0..l-1 + m[i%9,i/9] = i + 1 + end + assert m.approx?(m.dup) + end + + def test_dotV + m = Eigen::MatrixX.new(4,4) + 4.times { |i| m[i,i] = i + 1 } + a = Eigen::VectorX.from_a([1, 2, 3, 4]) + b = m.dotV(a) + expected = Eigen::VectorX.from_a([1, 4, 9, 16]) + assert_kind_of Eigen::VectorX, b + assert(expected.approx?(b)) + end + + def test_jacobisvd + m = Eigen::MatrixX.new(7,7) + 7.times { |i| m[i,i] = i + 1 } + solver = m.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV) + b = Eigen::VectorX.from_a([1, 2, 3, 4, 5, 6, 7]) + a = solver.solve(b) + + assert m.dotV(a).approx?(b) + end +end + diff --git a/test/quaternion_test.rb b/test/quaternion_test.rb new file mode 100644 index 0000000..8c818a7 --- /dev/null +++ b/test/quaternion_test.rb @@ -0,0 +1,86 @@ +require 'test_helper' + +class TC_Eigen_Quaternion < Minitest::Test + def test_base + v = Eigen::Quaternion.new(0, 1, 2, 3) + assert_equal(0, v.w) + assert_equal(1, v.x) + assert_equal(2, v.y) + assert_equal(3, v.z) + end + + def test_to_euler + q = Eigen::Quaternion.new(1, 0, 0, 0) + result = q.to_euler + assert_equal([0, 0, 0], result.to_a) + end + + def test_to_euler_to_quaternion + q = Eigen::Quaternion.new(0.2, 0.5, 0.1, 0.5) + q.normalize! + + v = q.to_euler + result = Eigen::Quaternion.from_euler(v, 2, 1, 0) + + assert(q.approx?(result, 0.0001)) + end + + def test_approx_returns_true_on_equality + q = Eigen::Quaternion.new(0, 0, 0, 0) + assert q.approx?(q) + end + + def test_approx_returns_true_on_quaternions_that_are_different_by_epsilon + q1 = Eigen::Quaternion.new(1, 1, 1, 1) + q2 = Eigen::Quaternion.new(1 + Float::EPSILON, 1, 1, 1) + assert q1.approx?(q2) + end + + def test_approx_returns_false_on_quaternions_that_are_different + q1 = Eigen::Quaternion.new(1, 1, 1, 1) + q2 = Eigen::Quaternion.new(2, 2, 2, 2) + refute q1.approx?(q2) + end + + def test_approx_returns_true_on_quaternions_that_are_less_different_than_the_proqided_accuracy + q1 = Eigen::Quaternion.new(1, 1, 1, 1) + q2 = Eigen::Quaternion.new(1.5, 1.5, 1.5, 1.5) + assert q1.approx?(q2, 2) + end + + def test_angle_axis + q = Eigen::Quaternion.from_angle_axis( Math::PI, Eigen::Vector3.new( 1, 0, 0 ) ) + v = Eigen::Vector3.new(0, 1, 0) + + assert( Eigen::Vector3.new(0, -1, 0).approx?( q*v, 0.0001 ) ) + end + + def test_inverse + q = Eigen::Quaternion.from_euler( Eigen::Vector3.new(0.1, 0, 0), 2, 1, 0) + q1 = Eigen::Quaternion.from_euler( Eigen::Vector3.new(-0.1, 0, 0), 2, 1, 0) + + assert(q.approx?( q1.inverse, 0.0001 )) + end + + def test_dump_load + q = Eigen::Quaternion.new(0.2, 0.5, 0.1, 0.5) + dumped = Marshal.dump(q) + loaded = Marshal.load(dumped) + assert(q.approx?(loaded, 0.0001)) + end + + def test_to_angle_axis + axis = (Eigen::Vector3.UnitX * 0.4 + Eigen::Vector3.UnitZ * 0.5).normalize + angle = 0.5 + q = Eigen::Quaternion.from_angle_axis(angle, axis) + result_angle, result_axis = q.to_angle_axis + assert_in_delta angle, result_angle, 1e-6 + assert result_axis.approx?(axis) + end + + def test_dup + q = Eigen::Quaternion.new(0.2, 0.5, 0.1, 0.5) + assert(q.dup.approx?(q, 0.0001)) + end +end + diff --git a/test/test_helper.rb b/test/test_helper.rb new file mode 100644 index 0000000..f6872cc --- /dev/null +++ b/test/test_helper.rb @@ -0,0 +1,54 @@ +$LOAD_PATH.unshift File.expand_path('../../lib', __FILE__) + +# simplecov must be loaded FIRST. Only the files required after it gets loaded +# will be profiled !!! +if ENV['TEST_ENABLE_COVERAGE'] == '1' + begin + require 'simplecov' + SimpleCov.start + rescue LoadError + require 'eigen' + warn "coverage is disabled because the 'simplecov' gem cannot be loaded" + rescue Exception => e + require 'eigen' + warn "coverage is disabled: #{e.message}" + end +end + +require 'eigen' +# require 'flexmock/minitest' +require 'minitest/autorun' + +if ENV['TEST_ENABLE_PRY'] != '0' + begin + require 'pry' + rescue Exception + warn "debugging is disabled because the 'pry' gem cannot be loaded" + end +end + +module Eigen + # This module is the common setup for all tests + # + # It should be included in the toplevel describe blocks + # + # @example + # require 'test_helper' + # describe Eigen do + # end + # + module SelfTest + def setup + super + end + + def teardown + super + end + end +end + +class Minitest::Test + extend Eigen::SelfTest +end + diff --git a/test/vector3_test.rb b/test/vector3_test.rb new file mode 100644 index 0000000..b3c84c3 --- /dev/null +++ b/test/vector3_test.rb @@ -0,0 +1,80 @@ +require 'test_helper' + +class TC_Eigen_Vector3 < Minitest::Test + def test_base + v = Eigen::Vector3.new(1, 2, 3) + assert_equal(1, v.x) + assert_equal(2, v.y) + assert_equal(3, v.z) + end + + def test_add + v0 = Eigen::Vector3.new(1, 2, 3) + v1 = Eigen::Vector3.new(-1, -2, -3) + assert_equal([0, 0, 0], (v0 + v1).to_a) + end + + def test_sub + v0 = Eigen::Vector3.new(1, 2, 3) + v1 = Eigen::Vector3.new(-1, -2, -3) + assert_equal([2, 4, 6], (v0 - v1).to_a) + end + + def test_opposite + v0 = Eigen::Vector3.new(1, 2, 3) + assert_equal([-1, -2, -3], (-v0).to_a) + end + + def test_scale + v0 = Eigen::Vector3.new(1, 2, 3) + assert_equal([2, 4, 6], (v0 * 2).to_a) + end + + def test_div_by_scalar + v0 = Eigen::Vector3.new(2, 4, 6) + assert_equal([1, 2, 3], (v0 / 2.0).to_a) + end + + def test_dump_load + v = Eigen::Vector3.new(0.2, 0.5, 0.1) + dumped = Marshal.dump(v) + loaded = Marshal.load(dumped) + assert v.approx?(loaded) + end + + def test_dup + v = Eigen::Vector3.new(0.2, 0.5, 0.1) + new = v.dup + assert v.approx?(new) + end + + def test_approx_returns_true_on_equal_vectors + v = Eigen::Vector3.new(0, 0, 0) + assert v.approx?(v) + end + + def test_approx_returns_true_on_vectors_that_are_different_by_epsilon + v1 = Eigen::Vector3.new(1, 1, 1) + v2 = Eigen::Vector3.new(1 + Float::EPSILON, 1, 1) + assert v1.approx?(v2) + end + + def test_approx_returns_false_on_vectors_that_are_different + v1 = Eigen::Vector3.new(1, 1, 1) + v2 = Eigen::Vector3.new(2, 2, 2) + refute v1.approx?(v2) + end + + def test_approx_returns_true_on_vectors_that_are_less_different_than_the_provided_accuracy + v1 = Eigen::Vector3.new(1, 1, 1) + v2 = Eigen::Vector3.new(1.5, 1.5, 1.5) + assert v1.approx?(v2, 2) + end + + def test_data_assign + v = Eigen::Vector3.new(0, 0, 0) + v.data = [1, 2, 3] + assert v.approx?(Eigen::Vector3.new(1, 2, 3)) + end +end +