eigen 0.1.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +7 -0
- data/.gitignore +14 -0
- data/.travis.yml +14 -0
- data/.yardopts +1 -0
- data/Gemfile +4 -0
- data/LICENSE.txt +21 -0
- data/README.md +45 -0
- data/Rakefile +18 -0
- data/bin/console +14 -0
- data/bin/setup +8 -0
- data/eigen.gemspec +27 -0
- data/ext/eigen/eigen.cpp +1170 -0
- data/ext/eigen/extconf.rb +10 -0
- data/lib/eigen.rb +15 -0
- data/lib/eigen/affine3.rb +35 -0
- data/lib/eigen/angle_axis.rb +90 -0
- data/lib/eigen/isometry3.rb +36 -0
- data/lib/eigen/matrix4.rb +96 -0
- data/lib/eigen/matrixx.rb +105 -0
- data/lib/eigen/quaternion.rb +253 -0
- data/lib/eigen/vector3.rb +178 -0
- data/lib/eigen/vectorx.rb +54 -0
- data/lib/eigen/version.rb +3 -0
- metadata +144 -0
checksums.yaml
ADDED
@@ -0,0 +1,7 @@
|
|
1
|
+
---
|
2
|
+
SHA1:
|
3
|
+
metadata.gz: a380f2fcb3da7e20495fb6d6f6878e4b6ed18cad
|
4
|
+
data.tar.gz: c2d14d50bd045111e1307a3a46d2345a108dfbb6
|
5
|
+
SHA512:
|
6
|
+
metadata.gz: 785fc65418e1f314508124301a2f4e1f13c3092d39d719ee6e6abbef4c5c67143797d87349658de70756c3f194094af108363baa905f5484b84ac3f521bd35a7
|
7
|
+
data.tar.gz: 2162b6fe2bdc552eef5fb761190d84b7b1bfd0e7fbf02dc8c551723dfcdd991ae1ea3553ee87bbb4dd1b566f9e947c303d6a00e4a3bc29e2a2ab6148d2284001
|
data/.gitignore
ADDED
data/.travis.yml
ADDED
data/.yardopts
ADDED
@@ -0,0 +1 @@
|
|
1
|
+
ext/**/*.cpp lib/**/*.rb
|
data/Gemfile
ADDED
data/LICENSE.txt
ADDED
@@ -0,0 +1,21 @@
|
|
1
|
+
The MIT License (MIT)
|
2
|
+
|
3
|
+
Copyright (c) 2016 Sylvain Joyeux
|
4
|
+
|
5
|
+
Permission is hereby granted, free of charge, to any person obtaining a copy
|
6
|
+
of this software and associated documentation files (the "Software"), to deal
|
7
|
+
in the Software without restriction, including without limitation the rights
|
8
|
+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
9
|
+
copies of the Software, and to permit persons to whom the Software is
|
10
|
+
furnished to do so, subject to the following conditions:
|
11
|
+
|
12
|
+
The above copyright notice and this permission notice shall be included in
|
13
|
+
all copies or substantial portions of the Software.
|
14
|
+
|
15
|
+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
16
|
+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
17
|
+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
18
|
+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
19
|
+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
20
|
+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
21
|
+
THE SOFTWARE.
|
data/README.md
ADDED
@@ -0,0 +1,45 @@
|
|
1
|
+
# Eigen
|
2
|
+
|
3
|
+
[![Build Status](https://travis-ci.org/rock-core/base-ruby_eigen.svg?branch=master)](https://travis-ci.org/rock-core/base-ruby_eigen)
|
4
|
+
[![Gem Version](https://badge.fury.io/rb/eigen.svg)](http://badge.fury.io/rb/eigen)
|
5
|
+
[![Documentation](http://b.repl.ca/v1/yard-docs-blue.png)](http://rubydoc.info/gems/eigen/frames)
|
6
|
+
|
7
|
+
This gem provides bindings for the
|
8
|
+
[Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) linear algebra
|
9
|
+
library. They are quite limited, to the methods / functionality that were
|
10
|
+
needed. Functionality gets added as it goes, there is no goal to have a 100%
|
11
|
+
coverage of the Eigen functionality.
|
12
|
+
|
13
|
+
## Installation
|
14
|
+
|
15
|
+
Add this line to your application's Gemfile:
|
16
|
+
|
17
|
+
```ruby
|
18
|
+
gem 'eigen'
|
19
|
+
```
|
20
|
+
|
21
|
+
And then execute:
|
22
|
+
|
23
|
+
$ bundle
|
24
|
+
|
25
|
+
Or install it yourself as:
|
26
|
+
|
27
|
+
$ gem install eigen
|
28
|
+
|
29
|
+
## Usage
|
30
|
+
|
31
|
+
## Development
|
32
|
+
|
33
|
+
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.
|
34
|
+
|
35
|
+
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).
|
36
|
+
|
37
|
+
## Contributing
|
38
|
+
|
39
|
+
Bug reports and pull requests are welcome on GitHub at https://github.com/rock-core/base-ruby_eigen
|
40
|
+
|
41
|
+
|
42
|
+
## License
|
43
|
+
|
44
|
+
The gem is available as open source under the terms of the [MIT License](http://opensource.org/licenses/MIT).
|
45
|
+
|
data/Rakefile
ADDED
@@ -0,0 +1,18 @@
|
|
1
|
+
require "bundler/gem_tasks"
|
2
|
+
require "rake/testtask"
|
3
|
+
|
4
|
+
Rake::TestTask.new(:test) do |t|
|
5
|
+
t.libs << "test"
|
6
|
+
t.libs << "lib"
|
7
|
+
t.test_files = FileList['test/**/*_test.rb']
|
8
|
+
end
|
9
|
+
|
10
|
+
require "rake/extensiontask"
|
11
|
+
|
12
|
+
task :build => :compile
|
13
|
+
|
14
|
+
Rake::ExtensionTask.new("eigen") do |ext|
|
15
|
+
ext.lib_dir = "lib/eigen"
|
16
|
+
end
|
17
|
+
|
18
|
+
task :default => :compile
|
data/bin/console
ADDED
@@ -0,0 +1,14 @@
|
|
1
|
+
#!/usr/bin/env ruby
|
2
|
+
|
3
|
+
require "bundler/setup"
|
4
|
+
require "eigen"
|
5
|
+
|
6
|
+
# You can add fixtures and/or initialization code here to make experimenting
|
7
|
+
# with your gem easier. You can also use a different console, if you like.
|
8
|
+
|
9
|
+
# (If you use this, don't forget to add pry to your Gemfile!)
|
10
|
+
# require "pry"
|
11
|
+
# Pry.start
|
12
|
+
|
13
|
+
require "irb"
|
14
|
+
IRB.start
|
data/bin/setup
ADDED
data/eigen.gemspec
ADDED
@@ -0,0 +1,27 @@
|
|
1
|
+
# coding: utf-8
|
2
|
+
lib = File.expand_path('../lib', __FILE__)
|
3
|
+
$LOAD_PATH.unshift(lib) unless $LOAD_PATH.include?(lib)
|
4
|
+
require 'eigen/version'
|
5
|
+
|
6
|
+
Gem::Specification.new do |spec|
|
7
|
+
spec.name = "eigen"
|
8
|
+
spec.version = Eigen::VERSION
|
9
|
+
spec.authors = ["Sylvain Joyeux"]
|
10
|
+
spec.email = ["sylvain.joyeux@m4x.org"]
|
11
|
+
|
12
|
+
spec.summary = %q{Ruby bindings to the Eigen C++ linear algebra library}
|
13
|
+
spec.homepage = "https://github.com/rock-core/base-ruby_eigen"
|
14
|
+
spec.license = "MIT"
|
15
|
+
|
16
|
+
spec.files = `git ls-files -z`.split("\x0").reject { |f| f.match(%r{^(test|spec|features)/}) }
|
17
|
+
spec.bindir = "exe"
|
18
|
+
spec.executables = spec.files.grep(%r{^exe/}) { |f| File.basename(f) }
|
19
|
+
spec.require_paths = ["lib"]
|
20
|
+
spec.extensions = ["ext/eigen/extconf.rb"]
|
21
|
+
|
22
|
+
spec.add_development_dependency 'rice', '~> 2.1', '>= 2.1.0'
|
23
|
+
spec.add_development_dependency "bundler", "~> 1.12"
|
24
|
+
spec.add_development_dependency "rake", "~> 10.0"
|
25
|
+
spec.add_development_dependency "rake-compiler"
|
26
|
+
spec.add_development_dependency "minitest", "~> 5.0"
|
27
|
+
end
|
data/ext/eigen/eigen.cpp
ADDED
@@ -0,0 +1,1170 @@
|
|
1
|
+
#include "rice/Class.hpp"
|
2
|
+
#include "rice/String.hpp"
|
3
|
+
#include "rice/Constructor.hpp"
|
4
|
+
#include "rice/Enum.hpp"
|
5
|
+
|
6
|
+
#include <Eigen/Core>
|
7
|
+
#include <Eigen/Geometry>
|
8
|
+
#include <Eigen/SVD>
|
9
|
+
|
10
|
+
using namespace Rice;
|
11
|
+
|
12
|
+
typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3d;
|
13
|
+
typedef Eigen::Matrix<double, 4, 4, Eigen::DontAlign> Matrix4d;
|
14
|
+
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaterniond;
|
15
|
+
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign>
|
16
|
+
MatrixXd;
|
17
|
+
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign>
|
18
|
+
VectorXd;
|
19
|
+
typedef Eigen::Transform< double, 3, Eigen::Isometry > Isometry3d;
|
20
|
+
typedef Eigen::Transform< double, 3, Eigen::Affine > Affine3d;
|
21
|
+
typedef Eigen::AngleAxis<double> AngleAxisd;
|
22
|
+
|
23
|
+
/*
|
24
|
+
* Document-class: Eigen::Vector3
|
25
|
+
*
|
26
|
+
* A 3-vector holding floating-point numbers
|
27
|
+
*
|
28
|
+
* @!method initialize(x = 0, y = 0, z = 0)
|
29
|
+
* Creates a new vector
|
30
|
+
* @param [Numeric] x
|
31
|
+
* @param [Numeric] y
|
32
|
+
* @param [Numeric] z
|
33
|
+
* @!method norm
|
34
|
+
* The vector's norm
|
35
|
+
* @return [Numeric]
|
36
|
+
* @!method normalize!
|
37
|
+
* Normalizes self
|
38
|
+
* @return [void]
|
39
|
+
* @!method normalize
|
40
|
+
* Returns a normalized self
|
41
|
+
* @return [Vector3]
|
42
|
+
* @!method [](index)
|
43
|
+
* Returns an element
|
44
|
+
* @param [Integer] index the element index (0, 1 or 2)
|
45
|
+
* @return [Numeric]
|
46
|
+
* @!method x
|
47
|
+
* Returns X
|
48
|
+
* @return [Numeric]
|
49
|
+
* @!method y
|
50
|
+
* Returns Y
|
51
|
+
* @return [Numeric]
|
52
|
+
* @!method z
|
53
|
+
* Returns Z
|
54
|
+
* @return [Numeric]
|
55
|
+
* @!method x=(value)
|
56
|
+
* Sets X
|
57
|
+
* @param [Numeric] value
|
58
|
+
* @return [Numeric]
|
59
|
+
* @!method y=(value)
|
60
|
+
* Sets Y
|
61
|
+
* @param [Numeric] value
|
62
|
+
* @return [Numeric]
|
63
|
+
* @!method z=(value)
|
64
|
+
* Sets Z
|
65
|
+
* @param [Numeric] value
|
66
|
+
* @return [Numeric]
|
67
|
+
* @!method []=(index, value)
|
68
|
+
* Sets an element
|
69
|
+
* @param [Integer] index the element index (0, 1 or 2)
|
70
|
+
* @param [Numeric] value
|
71
|
+
* @return [Numeric]
|
72
|
+
* @!method +(v)
|
73
|
+
* Sum
|
74
|
+
* @param [Vector3] v
|
75
|
+
* @return [Vector3] the sum
|
76
|
+
* @!method -(v)
|
77
|
+
* Subtracts
|
78
|
+
* @param [Vector3] v
|
79
|
+
* @return [Vector3] the subtraction
|
80
|
+
* @!method *(scalar)
|
81
|
+
* Multiplies by a scalar
|
82
|
+
* @param [Numeric] scalar the scalar
|
83
|
+
* @return [Vector3] the result
|
84
|
+
* @!method /(scalar)
|
85
|
+
* Divides by a scalar
|
86
|
+
* @param [Numeric] scalar the scalar
|
87
|
+
* @return [Vector3] the result
|
88
|
+
* @!method -@()
|
89
|
+
* Negation
|
90
|
+
* @return [Vector3] the result
|
91
|
+
* @!method cross(v)
|
92
|
+
* Cross product
|
93
|
+
* @param [VectorX] v
|
94
|
+
* @return [VectorX] the result
|
95
|
+
* @!method dot(v)
|
96
|
+
* Dot product
|
97
|
+
* @param [VectorX] v
|
98
|
+
* @return [VectorX] the result
|
99
|
+
* @!method approx?(v, threshold = dummy_precision)
|
100
|
+
* Verifies that two vectors are within threshold of each other, elementwise
|
101
|
+
* @param [Vector3]
|
102
|
+
* @return [Boolean]
|
103
|
+
*/
|
104
|
+
|
105
|
+
struct Vector3
|
106
|
+
{
|
107
|
+
Vector3d* v;
|
108
|
+
|
109
|
+
Vector3(double x, double y, double z)
|
110
|
+
: v(new Vector3d(x, y, z)) {}
|
111
|
+
Vector3(Vector3d const& _v)
|
112
|
+
: v(new Vector3d(_v)) {}
|
113
|
+
~Vector3()
|
114
|
+
{ delete v; }
|
115
|
+
|
116
|
+
double x() const { return v->x(); }
|
117
|
+
double y() const { return v->y(); }
|
118
|
+
double z() const { return v->z(); }
|
119
|
+
void setX(double value) { v->x() = value; }
|
120
|
+
void setY(double value) { v->y() = value; }
|
121
|
+
void setZ(double value) { v->z() = value; }
|
122
|
+
|
123
|
+
|
124
|
+
double norm() const { return v->norm(); }
|
125
|
+
Vector3* normalize() const { return new Vector3(v->normalized()); }
|
126
|
+
void normalizeBang() const { v->normalize(); }
|
127
|
+
|
128
|
+
double get(int i) const { return (*v)[i]; }
|
129
|
+
void set(int i, double value) { (*v)[i] = value; }
|
130
|
+
|
131
|
+
Vector3* operator + (Vector3 const& other) const
|
132
|
+
{ return new Vector3(*v + *other.v); }
|
133
|
+
Vector3* operator - (Vector3 const& other) const
|
134
|
+
{ return new Vector3(*v - *other.v); }
|
135
|
+
|
136
|
+
Vector3* operator / (double scalar) const
|
137
|
+
{ return new Vector3(*v / scalar); }
|
138
|
+
|
139
|
+
Vector3* negate() const
|
140
|
+
{ return new Vector3(-*v); }
|
141
|
+
Vector3* scale(double value) const
|
142
|
+
{ return new Vector3(*v * value); }
|
143
|
+
double dot(Vector3 const& other) const
|
144
|
+
{ return this->v->dot(*other.v); }
|
145
|
+
Vector3* cross(Vector3 const& other) const
|
146
|
+
{ return new Vector3(this->v->cross(*other.v)); }
|
147
|
+
bool operator ==(Vector3 const& other) const
|
148
|
+
{ return (*this->v) == (*other.v); }
|
149
|
+
bool isApprox(Vector3 const& other, double tolerance)
|
150
|
+
{ return v->isApprox(*other.v, tolerance); }
|
151
|
+
};
|
152
|
+
|
153
|
+
/*
|
154
|
+
* Document-class: Eigen::VectorX
|
155
|
+
*
|
156
|
+
* A variable-length vector holding floating-point numbers
|
157
|
+
*
|
158
|
+
* @!method initialize(size = 0)
|
159
|
+
* Creates a new vector
|
160
|
+
* @param [Numeric] size
|
161
|
+
* @!method resize(new_size)
|
162
|
+
* Changes the vector's size
|
163
|
+
* @param [Integer] new_size
|
164
|
+
* @return [Numeric]
|
165
|
+
* @!method norm
|
166
|
+
* The vector's norm
|
167
|
+
* @return [Numeric]
|
168
|
+
* @!method normalize!
|
169
|
+
* Normalizes self
|
170
|
+
* @return [void]
|
171
|
+
* @!method normalize
|
172
|
+
* Returns a normalized self
|
173
|
+
* @return [VectorX]
|
174
|
+
* @!method size
|
175
|
+
* Returns the vector's size
|
176
|
+
* @return [Integer]
|
177
|
+
* @!method [](index)
|
178
|
+
* Returns an element
|
179
|
+
* @param [Integer] index the element index (0, 1 or 2)
|
180
|
+
* @return [Numeric]
|
181
|
+
* @!method []=(index, value)
|
182
|
+
* Sets an element
|
183
|
+
* @param [Integer] index the element index (0, 1 or 2)
|
184
|
+
* @param [Numeric] value
|
185
|
+
* @return [Numeric]
|
186
|
+
* @!method +(v)
|
187
|
+
* Sum
|
188
|
+
* @param [VectorX] v
|
189
|
+
* @return [VectorX] the sum
|
190
|
+
* @!method -(v)
|
191
|
+
* Subtracts
|
192
|
+
* @param [VectorX] v
|
193
|
+
* @return [VectorX] the subtraction
|
194
|
+
* @!method *(scalar)
|
195
|
+
* Multiplies by a scalar
|
196
|
+
* @param [Numeric] scalar the scalar
|
197
|
+
* @return [VectorX] the result
|
198
|
+
* @!method /(scalar)
|
199
|
+
* Divides by a scalar
|
200
|
+
* @param [Numeric] scalar the scalar
|
201
|
+
* @return [VectorX] the result
|
202
|
+
* @!method -@()
|
203
|
+
* Negation
|
204
|
+
* @return [VectorX] the result
|
205
|
+
* @!method dot(v)
|
206
|
+
* Dot product
|
207
|
+
* @param [VectorX] v
|
208
|
+
* @return [VectorX] the result
|
209
|
+
* @!method approx?(v, threshold = dummy_precision)
|
210
|
+
* Verifies that two vectors are within threshold of each other, elementwise
|
211
|
+
* @param [VectorX]
|
212
|
+
* @return [Boolean]
|
213
|
+
*/
|
214
|
+
struct VectorX {
|
215
|
+
|
216
|
+
VectorXd* v;
|
217
|
+
|
218
|
+
VectorX()
|
219
|
+
: v(new VectorXd()) {}
|
220
|
+
VectorX(VectorX const& v)
|
221
|
+
: v(new VectorXd(*v.v)) {}
|
222
|
+
VectorX(int n)
|
223
|
+
: v(new VectorXd(n)) {}
|
224
|
+
VectorX(VectorXd const& _v)
|
225
|
+
: v(new VectorXd(_v)) {}
|
226
|
+
~VectorX()
|
227
|
+
{ delete v; }
|
228
|
+
|
229
|
+
void resize(int n) { v->resize(n); }
|
230
|
+
void conservativeResize(int n) { v->conservativeResize(n); }
|
231
|
+
|
232
|
+
double norm() const { return v->norm(); }
|
233
|
+
VectorX* normalize() const { return new VectorX(v->normalized()); }
|
234
|
+
void normalizeBang() const { v->normalize(); }
|
235
|
+
|
236
|
+
unsigned int size() { return v->size(); }
|
237
|
+
|
238
|
+
double get(int i) const { return (*v)[i]; }
|
239
|
+
void set(int i, double value) { (*v)[i] = value; }
|
240
|
+
|
241
|
+
VectorX* operator + (VectorX const& other) const
|
242
|
+
{ return new VectorX(*v + *other.v); }
|
243
|
+
VectorX* operator - (VectorX const& other) const
|
244
|
+
{ return new VectorX(*v - *other.v); }
|
245
|
+
|
246
|
+
VectorX* operator / (double scalar) const
|
247
|
+
{ return new VectorX(*v / scalar); }
|
248
|
+
|
249
|
+
VectorX* negate() const
|
250
|
+
{ return new VectorX(-*v); }
|
251
|
+
|
252
|
+
VectorX* scale(double value) const
|
253
|
+
{ return new VectorX(*v * value); }
|
254
|
+
|
255
|
+
double dot(VectorX const& other) const
|
256
|
+
{ return this->v->dot(*other.v); }
|
257
|
+
|
258
|
+
bool operator ==(VectorX const& other) const
|
259
|
+
{ return (*this->v) == (*other.v); }
|
260
|
+
|
261
|
+
bool isApprox(VectorX const& other, double tolerance)
|
262
|
+
{ return v->isApprox(*other.v, tolerance); }
|
263
|
+
|
264
|
+
};
|
265
|
+
|
266
|
+
/*
|
267
|
+
* Document-class: Eigen::Matrix4
|
268
|
+
*
|
269
|
+
* A 4x4 matrix holding floating-point numbers
|
270
|
+
*
|
271
|
+
* @!method rows
|
272
|
+
* @return [Integer] the number of rows
|
273
|
+
* @!method cols
|
274
|
+
* @return [Numeric] the number of columns
|
275
|
+
* @!method size
|
276
|
+
* @return [Numeric] the number of elements
|
277
|
+
* @!method [](row, col)
|
278
|
+
* Accesses an element
|
279
|
+
* @param [Integer] row the element's row
|
280
|
+
* @param [Integer] col the element's column
|
281
|
+
* @return [Numeric] the required element
|
282
|
+
* @!method []=(row, col, value)
|
283
|
+
* Sets an element
|
284
|
+
* @param [Integer] row the element's row
|
285
|
+
* @param [Integer] col the element's column
|
286
|
+
* @param [Numeric] value the new value
|
287
|
+
* @return [Numeric] the value
|
288
|
+
* @!method +(m)
|
289
|
+
* Sums two matrices
|
290
|
+
* @param [Matrix4] m the matrix to add
|
291
|
+
* @return [Matrix4] the sum
|
292
|
+
* @!method -(m)
|
293
|
+
* Subtracts two matrices
|
294
|
+
* @param [Matrix4] m the matrix to subtract to self
|
295
|
+
* @return [Matrix4] the subtraction
|
296
|
+
* @!method *(v)
|
297
|
+
* Multiplies by a scalar
|
298
|
+
* @param [Numeric] v the scalar
|
299
|
+
* @return [Matrix4] the result
|
300
|
+
* @!method /(v)
|
301
|
+
* Divides this matrix by a scalar
|
302
|
+
* @param [Numeric] v the scalar
|
303
|
+
* @return [Matrix4] the result
|
304
|
+
* @!method -@(v)
|
305
|
+
* Returns this matrix' negation
|
306
|
+
* @return [Matrix4] the result
|
307
|
+
* @!method T
|
308
|
+
* Returns the transposed matrix
|
309
|
+
* @return [Matrix4]
|
310
|
+
* @!method norm
|
311
|
+
* Returns the matrix' norm
|
312
|
+
* @return [Numeric]
|
313
|
+
* @!method dotM(m)
|
314
|
+
* Matrix multiplication
|
315
|
+
* @param [Matrix4]
|
316
|
+
* @return [Numeric]
|
317
|
+
* @!method approx?(m, threshold = dummy_precision)
|
318
|
+
* Verifies that two matrices are within threshold of each other, elementwise
|
319
|
+
* @param [Matrix4]
|
320
|
+
* @return [Boolean]
|
321
|
+
*/
|
322
|
+
struct Matrix4
|
323
|
+
{
|
324
|
+
Matrix4d* mx;
|
325
|
+
|
326
|
+
Matrix4() : mx(new Matrix4d()) {}
|
327
|
+
|
328
|
+
Matrix4(Matrix4d const& _mx)
|
329
|
+
: mx(new Matrix4d(_mx)) {}
|
330
|
+
|
331
|
+
~Matrix4()
|
332
|
+
{ delete mx; }
|
333
|
+
|
334
|
+
double norm() const { return mx->norm(); }
|
335
|
+
|
336
|
+
int rows() const { return mx->rows(); }
|
337
|
+
int cols() const { return mx->cols(); }
|
338
|
+
int size() const { return mx->size(); }
|
339
|
+
|
340
|
+
double get(int i, int j ) const { return (*mx)(i,j); }
|
341
|
+
void set(int i, int j, double value) { (*mx)(i,j) = value; }
|
342
|
+
|
343
|
+
Matrix4* transpose() const
|
344
|
+
{ return new Matrix4(mx->transpose()); }
|
345
|
+
|
346
|
+
Matrix4* operator + (Matrix4 const& other) const
|
347
|
+
{ return new Matrix4(*mx + *other.mx); }
|
348
|
+
|
349
|
+
Matrix4* operator - (Matrix4 const& other) const
|
350
|
+
{ return new Matrix4(*mx - *other.mx); }
|
351
|
+
|
352
|
+
Matrix4* operator / (double scalar) const
|
353
|
+
{ return new Matrix4(*mx / scalar); }
|
354
|
+
|
355
|
+
Matrix4* negate() const
|
356
|
+
{ return new Matrix4(-*mx); }
|
357
|
+
|
358
|
+
Matrix4* scale(double value) const
|
359
|
+
{ return new Matrix4(*mx * value); }
|
360
|
+
|
361
|
+
Matrix4* dotM (Matrix4 const& other) const
|
362
|
+
{ return new Matrix4(*mx * (*other.mx)); }
|
363
|
+
|
364
|
+
bool operator ==(Matrix4 const& other) const
|
365
|
+
{ return (*this->mx) == (*other.mx); }
|
366
|
+
|
367
|
+
bool isApprox(Matrix4 const& other, double tolerance)
|
368
|
+
{ return mx->isApprox(*other.mx, tolerance); }
|
369
|
+
};
|
370
|
+
|
371
|
+
/*
|
372
|
+
* Document-class: Eigen::JacobiSVD
|
373
|
+
*
|
374
|
+
* Linear problem solver
|
375
|
+
*
|
376
|
+
* This is not constructed directly. Use {Eigen::MatrixX#jacobiSvd} instead.
|
377
|
+
*
|
378
|
+
* @!method solve(vector)
|
379
|
+
* Solves the linear problem for a given vector
|
380
|
+
* @param [VectorX] vector
|
381
|
+
* @return [VectorX] the result
|
382
|
+
*/
|
383
|
+
struct JacobiSVD {
|
384
|
+
typedef Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject> EigenT;
|
385
|
+
EigenT* j;
|
386
|
+
|
387
|
+
JacobiSVD( EigenT const& j )
|
388
|
+
: j(new EigenT(j)) {}
|
389
|
+
JacobiSVD( JacobiSVD const& j )
|
390
|
+
: j(new EigenT(*j.j)) {}
|
391
|
+
~JacobiSVD()
|
392
|
+
{ delete j; }
|
393
|
+
|
394
|
+
VectorX* solve(VectorX* y)
|
395
|
+
{ return new VectorX(j->solve(*y->v)); }
|
396
|
+
};
|
397
|
+
|
398
|
+
/*
|
399
|
+
* Document-class: Eigen::MatrixX
|
400
|
+
*
|
401
|
+
* A variable-size matrix holding floating-point numbers
|
402
|
+
*
|
403
|
+
* @!method resize(rows, cols)
|
404
|
+
* Resizes the matrix
|
405
|
+
* @param [Integer] rows the new number of rows
|
406
|
+
* @param [Integer] cols the new number of columns
|
407
|
+
* @!method rows
|
408
|
+
* @return [Integer] the number of rows
|
409
|
+
* @!method cols
|
410
|
+
* @return [Numeric] the number of columns
|
411
|
+
* @!method size
|
412
|
+
* @return [Numeric] the number of elements
|
413
|
+
* @!method [](row, col)
|
414
|
+
* Accesses an element
|
415
|
+
* @param [Integer] row the element's row
|
416
|
+
* @param [Integer] col the element's column
|
417
|
+
* @return [Numeric] the required element
|
418
|
+
* @!method []=(row, col, value)
|
419
|
+
* Sets an element
|
420
|
+
* @param [Integer] row the element's row
|
421
|
+
* @param [Integer] col the element's column
|
422
|
+
* @param [Numeric] value the new value
|
423
|
+
* @return [Numeric] the value
|
424
|
+
* @!method setRow(row, vector)
|
425
|
+
* Sets a whole matrix row
|
426
|
+
* @param [Integer] row the row index
|
427
|
+
* @param [VectorX] vector the row values
|
428
|
+
* @!method setColumn(column, vector)
|
429
|
+
* Sets a whole matrix column
|
430
|
+
* @param [Integer] row the column index
|
431
|
+
* @param [VectorX] vector the column values
|
432
|
+
* @!method +(m)
|
433
|
+
* Sums two matrices
|
434
|
+
* @param [MatrixX] m the matrix to add
|
435
|
+
* @return [MatrixX] the sum
|
436
|
+
* @!method -(m)
|
437
|
+
* Subtracts two matrices
|
438
|
+
* @param [MatrixX] m the matrix to subtract to self
|
439
|
+
* @return [MatrixX] the subtraction
|
440
|
+
* @!method *(v)
|
441
|
+
* Multiplies by a scalar
|
442
|
+
* @param [Numeric] v the scalar
|
443
|
+
* @return [MatrixX] the result
|
444
|
+
* @!method /(v)
|
445
|
+
* Divides this matrix by a scalar
|
446
|
+
* @param [Numeric] v the scalar
|
447
|
+
* @return [MatrixX] the result
|
448
|
+
* @!method -@(v)
|
449
|
+
* Returns this matrix' negation
|
450
|
+
* @return [MatrixX] the result
|
451
|
+
* @!method T
|
452
|
+
* Returns the transposed matrix
|
453
|
+
* @return [MatrixX]
|
454
|
+
* @!method norm
|
455
|
+
* Returns the matrix' norm
|
456
|
+
* @return [Numeric]
|
457
|
+
* @!method dotV(m)
|
458
|
+
* Matrix/vector multiplication
|
459
|
+
* @param [VectorX]
|
460
|
+
* @return [Numeric]
|
461
|
+
* @!method dotM(m)
|
462
|
+
* Matrix multiplication
|
463
|
+
* @param [Matrix4]
|
464
|
+
* @return [Numeric]
|
465
|
+
* @!method approx?(m, threshold = dummy_precision)
|
466
|
+
* Verifies that two matrices are within threshold of each other, elementwise
|
467
|
+
* @param [Matrix4]
|
468
|
+
* @return [Boolean]
|
469
|
+
* @!method jacobiSvd(flags = 0)
|
470
|
+
* Returns a SVD solver to find V from W in self.dotV(V) = W
|
471
|
+
* @param [Integer] flags solver flags, as OR-ed values of Eigen::ComputeFullU,
|
472
|
+
* Eigen::ComputeThinU and Eigen::ComputeThinV. See Eigen documentation
|
473
|
+
* @return [JacobiSVD]
|
474
|
+
*/
|
475
|
+
struct MatrixX {
|
476
|
+
|
477
|
+
MatrixXd* m;
|
478
|
+
|
479
|
+
MatrixX() : m(new MatrixXd()) {}
|
480
|
+
MatrixX(const MatrixX& m) : m(new MatrixXd(*m.m)) {}
|
481
|
+
MatrixX(int rows, int cols) : m(new MatrixXd(rows,cols)) {}
|
482
|
+
MatrixX(const MatrixXd& _m) : m(new MatrixXd(_m)) {}
|
483
|
+
~MatrixX() { delete m; }
|
484
|
+
|
485
|
+
void resize(int rows, int cols) { m->resize(rows,cols); }
|
486
|
+
void conservativeResize(int rows, int cols) { m->conservativeResize(rows,cols); }
|
487
|
+
|
488
|
+
double norm() const { return m->norm(); }
|
489
|
+
|
490
|
+
unsigned int rows() const { return m->rows(); }
|
491
|
+
unsigned int cols() const { return m->cols(); }
|
492
|
+
unsigned int size() const { return m->size(); }
|
493
|
+
|
494
|
+
double get(int i, int j ) const { return (*m)(i,j); }
|
495
|
+
void set(int i, int j, double value) { (*m)(i,j) = value; }
|
496
|
+
|
497
|
+
VectorX* getRow(int i) const { return new VectorX(m->row(i)); }
|
498
|
+
void setRow(int i, const VectorX& v) { m->row(i) = *(v.v); }
|
499
|
+
|
500
|
+
VectorX* getColumn(int j) const { return new VectorX(m->col(j)); }
|
501
|
+
void setColumn(int j, const VectorX& v) { m->col(j) = *(v.v); }
|
502
|
+
|
503
|
+
MatrixX* transpose() const
|
504
|
+
{ return new MatrixX(m->transpose()); }
|
505
|
+
|
506
|
+
MatrixX* operator + (MatrixX const& other) const
|
507
|
+
{ return new MatrixX(*m + *other.m); }
|
508
|
+
|
509
|
+
MatrixX* operator - (MatrixX const& other) const
|
510
|
+
{ return new MatrixX(*m - *other.m); }
|
511
|
+
|
512
|
+
MatrixX* operator / (double scalar) const
|
513
|
+
{ return new MatrixX(*m / scalar); }
|
514
|
+
|
515
|
+
MatrixX* negate() const
|
516
|
+
{ return new MatrixX(-*m); }
|
517
|
+
|
518
|
+
MatrixX* scale (double scalar) const
|
519
|
+
{ return new MatrixX(*m * scalar); }
|
520
|
+
|
521
|
+
VectorX* dotV (VectorX const& other) const
|
522
|
+
{ return new VectorX(*m * *other.v); }
|
523
|
+
|
524
|
+
MatrixX* dotM (MatrixX const& other) const
|
525
|
+
{ return new MatrixX(*m * (*other.m)); }
|
526
|
+
|
527
|
+
JacobiSVD* jacobiSvd(int flags = 0) const
|
528
|
+
{ return new JacobiSVD(m->jacobiSvd(flags)); }
|
529
|
+
|
530
|
+
bool operator ==(MatrixX const& other) const
|
531
|
+
{ return (*this->m) == (*other.m); }
|
532
|
+
|
533
|
+
bool isApprox(MatrixX const& other, double tolerance)
|
534
|
+
{ return m->isApprox(*other.m, tolerance); }
|
535
|
+
};
|
536
|
+
|
537
|
+
/*
|
538
|
+
* Document-class: Eigen::Quaternion
|
539
|
+
*
|
540
|
+
* A floating-point valued quaternion
|
541
|
+
*
|
542
|
+
* @!method w
|
543
|
+
* The real part of the quaternion
|
544
|
+
* @return [Numeric]
|
545
|
+
* @!method x
|
546
|
+
* The first element of the imaginary part of the quaternion
|
547
|
+
* @return [Numeric]
|
548
|
+
* @!method y
|
549
|
+
* The second element of the imaginary part of the quaternion
|
550
|
+
* @return [Numeric]
|
551
|
+
* @!method z
|
552
|
+
* The third element of the imaginary part of the quaternion
|
553
|
+
* @return [Numeric]
|
554
|
+
* @!method w=(value)
|
555
|
+
* Sets the real part of the quaternion
|
556
|
+
* @param [Numeric] value
|
557
|
+
* @return [Numeric]
|
558
|
+
* @!method x=(value)
|
559
|
+
* Sets the first element of the imaginary part of the quaternion
|
560
|
+
* @param [Numeric] value
|
561
|
+
* @return [Numeric]
|
562
|
+
* @!method y=(value)
|
563
|
+
* Sets the second element of the imaginary part of the quaternion
|
564
|
+
* @param [Numeric] value
|
565
|
+
* @return [Numeric]
|
566
|
+
* @!method z=(value)
|
567
|
+
* Sets the third element of the imaginary part of the quaternion
|
568
|
+
* @param [Numeric] value
|
569
|
+
* @return [Numeric]
|
570
|
+
* @!method norm
|
571
|
+
* The norm
|
572
|
+
* @return [Numeric]
|
573
|
+
* @!method concatenate(q)
|
574
|
+
* Quaternion multiplication
|
575
|
+
* @param [Quaternion] q
|
576
|
+
* @return [Quaternion] self * q
|
577
|
+
* @!method inverse
|
578
|
+
* The quaternion inverse
|
579
|
+
* @return [Quaternion]
|
580
|
+
* @!method transform(v)
|
581
|
+
* Transform a vector-3 by this quaternion
|
582
|
+
* @param [Vector3] v
|
583
|
+
* @return [Vector3]
|
584
|
+
* @!method matrix
|
585
|
+
* The rotation matrix equivalent to this unit quaternion
|
586
|
+
* @return [MatrixX]
|
587
|
+
* @!method normalize!
|
588
|
+
* Normalizes self
|
589
|
+
* @return [void]
|
590
|
+
* @!method normalize
|
591
|
+
* Returns the self normalized
|
592
|
+
* @return [Quaternion]
|
593
|
+
* @!method approx?(q, threshold = dummy_precision)
|
594
|
+
* Verifies that two quaternions are within threshold of each other, elementwise
|
595
|
+
* @param [Quaternion]
|
596
|
+
* @return [Boolean]
|
597
|
+
* @!method to_euler
|
598
|
+
* Converts this quaternion into Euler-Bryant angles
|
599
|
+
* @return [Vector3] a 3-vector where .x is roll (rotation around X), .y is
|
600
|
+
* pitch and .z yaw
|
601
|
+
* @!method from_euler(v)
|
602
|
+
* Initializes from euler angles
|
603
|
+
* @param [Vector3] v a 3-vector where .x is roll (rotation around X), .y is
|
604
|
+
* pitch and .z yaw
|
605
|
+
* @return [void]
|
606
|
+
* @!method from_angle_axis(angle_axis)
|
607
|
+
* Initializes from an angle-axis representation
|
608
|
+
* @param [AngleAxis] an angle-axis representation of a rotation
|
609
|
+
* @return [void]
|
610
|
+
* @!method from_matrix(matrix)
|
611
|
+
* Initializes from a rotation matrix
|
612
|
+
* @param [MatrixX]
|
613
|
+
* @return [void]
|
614
|
+
*/
|
615
|
+
struct Quaternion
|
616
|
+
{
|
617
|
+
Quaterniond* q;
|
618
|
+
Quaternion(double w, double x, double y, double z)
|
619
|
+
: q(new Quaterniond(w, x, y, z)) { }
|
620
|
+
Quaternion(Quaternion const& q)
|
621
|
+
: q(new Quaterniond(*q.q)) { }
|
622
|
+
Quaternion(Quaterniond const& _q)
|
623
|
+
: q(new Quaterniond(_q)) {}
|
624
|
+
|
625
|
+
~Quaternion()
|
626
|
+
{ delete q; }
|
627
|
+
|
628
|
+
double w() const { return q->w(); }
|
629
|
+
double x() const { return q->x(); }
|
630
|
+
double y() const { return q->y(); }
|
631
|
+
double z() const { return q->z(); }
|
632
|
+
void setW(double value) { q->w() = value; }
|
633
|
+
void setX(double value) { q->x() = value; }
|
634
|
+
void setY(double value) { q->y() = value; }
|
635
|
+
void setZ(double value) { q->z() = value; }
|
636
|
+
|
637
|
+
double norm() const { return q->norm(); }
|
638
|
+
|
639
|
+
bool operator ==(Quaternion const& other) const
|
640
|
+
{ return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w(); }
|
641
|
+
|
642
|
+
Quaternion* concatenate(Quaternion const& other) const
|
643
|
+
{ return new Quaternion((*q) * (*other.q)); }
|
644
|
+
Vector3* transform(Vector3 const& v) const
|
645
|
+
{ return new Vector3((*q) * (*v.v)); }
|
646
|
+
Quaternion* inverse() const
|
647
|
+
{ return new Quaternion(q->inverse()); }
|
648
|
+
void normalizeBang()
|
649
|
+
{ q->normalize(); }
|
650
|
+
Quaternion* normalize() const
|
651
|
+
{
|
652
|
+
Quaterniond q = *this->q;
|
653
|
+
q.normalize();
|
654
|
+
return new Quaternion(q);
|
655
|
+
}
|
656
|
+
MatrixX* matrix() const
|
657
|
+
{
|
658
|
+
return new MatrixX(q->matrix());
|
659
|
+
}
|
660
|
+
|
661
|
+
void fromAngleAxis(double angle, Vector3 const& axis)
|
662
|
+
{
|
663
|
+
*(this->q) =
|
664
|
+
Eigen::AngleAxisd(angle, *axis.v);
|
665
|
+
}
|
666
|
+
|
667
|
+
void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2)
|
668
|
+
{
|
669
|
+
*(this->q) =
|
670
|
+
Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) *
|
671
|
+
Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) *
|
672
|
+
Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2));
|
673
|
+
}
|
674
|
+
|
675
|
+
void fromMatrix(MatrixX const& matrix)
|
676
|
+
{
|
677
|
+
*(this->q) =
|
678
|
+
Quaterniond(Eigen::Matrix3d(*matrix.m));
|
679
|
+
}
|
680
|
+
|
681
|
+
bool isApprox(Quaternion const& other, double tolerance)
|
682
|
+
{
|
683
|
+
return q->isApprox(*other.q, tolerance);
|
684
|
+
}
|
685
|
+
|
686
|
+
Vector3* toEuler()
|
687
|
+
{
|
688
|
+
const Eigen::Matrix3d m = q->toRotationMatrix();
|
689
|
+
double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
|
690
|
+
double y = atan2(-m.coeff(2,0), i);
|
691
|
+
double x=0,z=0;
|
692
|
+
if (i > Eigen::NumTraits<double>::dummy_precision()){
|
693
|
+
x = ::atan2(m.coeff(1,0), m.coeff(0,0));
|
694
|
+
z = ::atan2(m.coeff(2,1), m.coeff(2,2));
|
695
|
+
}else{
|
696
|
+
z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
|
697
|
+
}
|
698
|
+
return new Vector3(x,y,z);
|
699
|
+
}
|
700
|
+
};
|
701
|
+
|
702
|
+
|
703
|
+
/*
|
704
|
+
* Document-class: Eigen::AngleAxis
|
705
|
+
*
|
706
|
+
* A rotation represented by an axis and angle
|
707
|
+
*
|
708
|
+
* @!method angle
|
709
|
+
* The angle (in radians)
|
710
|
+
* @return [Numeric]
|
711
|
+
* @!method axis
|
712
|
+
* The rotation axis
|
713
|
+
* @return [Vector3]
|
714
|
+
* @!method concatenate(aa)
|
715
|
+
* Combine this rotation with another rotation
|
716
|
+
* @param [AngleAxis] aa
|
717
|
+
* @return [AngleAxis]
|
718
|
+
* @!method inverse
|
719
|
+
* The inverse rotation
|
720
|
+
* @return [AngleAxis]
|
721
|
+
* @!method transform(v)
|
722
|
+
* Apply this rotation on a 3-vector
|
723
|
+
* @param [Vector3] v
|
724
|
+
* @return [Vector3]
|
725
|
+
* @!method matrix
|
726
|
+
* The rotation matrix equivalent to this unit quaternion
|
727
|
+
* @return [MatrixX]
|
728
|
+
* @!method approx?(aa, threshold = dummy_precision)
|
729
|
+
* Verifies that two angle-axis are within threshold of each other, elementwise
|
730
|
+
* @param [AngleAxis]
|
731
|
+
* @return [Boolean]
|
732
|
+
* @!method to_euler
|
733
|
+
* Converts this quaternion into Euler-Bryant angles
|
734
|
+
* @return [Vector3] a 3-vector where .x is roll (rotation around X), .y is
|
735
|
+
* pitch and .z yaw
|
736
|
+
* @!method from_euler(v)
|
737
|
+
* Initializes from euler angles
|
738
|
+
* @param [Vector3] v a 3-vector where .x is roll (rotation around X), .y is
|
739
|
+
* pitch and .z yaw
|
740
|
+
* @!method from_quaternion(q)
|
741
|
+
* Initializes from a quaternion
|
742
|
+
* @param [Quaternion] q
|
743
|
+
* @!method from_matrix(matrix)
|
744
|
+
* Initializes from a rotation matrix
|
745
|
+
* @param [MatrixX]
|
746
|
+
*/
|
747
|
+
struct AngleAxis
|
748
|
+
{
|
749
|
+
AngleAxisd* aa;
|
750
|
+
AngleAxis(double angle, Vector3 const& axis)
|
751
|
+
: aa(new AngleAxisd(angle, Eigen::Vector3d(*axis.v))){}
|
752
|
+
AngleAxis(AngleAxis const& aa)
|
753
|
+
: aa(new AngleAxisd(*aa.aa)) { }
|
754
|
+
AngleAxis(AngleAxisd const& _aa)
|
755
|
+
: aa(new AngleAxisd(_aa)) {}
|
756
|
+
|
757
|
+
~AngleAxis()
|
758
|
+
{ delete aa; }
|
759
|
+
|
760
|
+
bool operator ==(AngleAxis const& other) const
|
761
|
+
{ return angle() == other.angle() && axis() == other.axis(); }
|
762
|
+
|
763
|
+
double angle() const { return aa->angle(); }
|
764
|
+
Vector3* axis() const { return new Vector3(aa->axis()); }
|
765
|
+
|
766
|
+
AngleAxis* concatenate(AngleAxis const& other) const
|
767
|
+
{ return new AngleAxis(static_cast<AngleAxisd>((*aa) * (*other.aa))); }
|
768
|
+
|
769
|
+
Vector3* transform(Vector3 const& v) const
|
770
|
+
{ return new Vector3((*aa) * (*v.v)); }
|
771
|
+
|
772
|
+
AngleAxis* inverse() const
|
773
|
+
{ return new AngleAxis(aa->inverse()); }
|
774
|
+
|
775
|
+
MatrixX* matrix() const
|
776
|
+
{
|
777
|
+
return new MatrixX(aa->matrix());
|
778
|
+
}
|
779
|
+
|
780
|
+
void fromQuaternion(Quaternion const& q)
|
781
|
+
{
|
782
|
+
*(this->aa) = Eigen::Quaterniond(q.w(), q.x(), q.y(), q.z());
|
783
|
+
}
|
784
|
+
|
785
|
+
void fromEuler(Vector3 const& angles, int axis0, int axis1, int axis2)
|
786
|
+
{
|
787
|
+
*(this->aa) =
|
788
|
+
Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::Unit(axis0)) *
|
789
|
+
Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::Unit(axis1)) *
|
790
|
+
Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::Unit(axis2));
|
791
|
+
}
|
792
|
+
|
793
|
+
void fromMatrix(MatrixX const& matrix)
|
794
|
+
{
|
795
|
+
*(this->aa) =
|
796
|
+
AngleAxisd(Eigen::Matrix3d(*matrix.m));
|
797
|
+
}
|
798
|
+
|
799
|
+
bool isApprox(AngleAxis const& other, double tolerance)
|
800
|
+
{
|
801
|
+
return aa->isApprox(*other.aa, tolerance);
|
802
|
+
}
|
803
|
+
|
804
|
+
Vector3* toEuler()
|
805
|
+
{
|
806
|
+
const Eigen::Matrix3d m = aa->toRotationMatrix();
|
807
|
+
double i = Eigen::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
|
808
|
+
double y = atan2(-m.coeff(2,0), i);
|
809
|
+
double x=0,z=0;
|
810
|
+
if (i > Eigen::NumTraits<double>::dummy_precision()){
|
811
|
+
x = ::atan2(m.coeff(1,0), m.coeff(0,0));
|
812
|
+
z = ::atan2(m.coeff(2,1), m.coeff(2,2));
|
813
|
+
}else{
|
814
|
+
z = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
|
815
|
+
}
|
816
|
+
return new Vector3(x,y,z);
|
817
|
+
}
|
818
|
+
};
|
819
|
+
|
820
|
+
#include <iostream>
|
821
|
+
|
822
|
+
/*
|
823
|
+
* Document-class: Eigen::Isometry3
|
824
|
+
*
|
825
|
+
* An isometry
|
826
|
+
*
|
827
|
+
* @!method approx?(v, threshold = dummy_precision)
|
828
|
+
* Verifies that two isometries are within threshold of each other, elementwise
|
829
|
+
* @param [Isometry3]
|
830
|
+
* @return [Boolean]
|
831
|
+
* @!method inverse
|
832
|
+
* The inverse transformation
|
833
|
+
* @return [Isometry3]
|
834
|
+
* @!method translation
|
835
|
+
* The translation part of this transformation
|
836
|
+
* @return [Vector3]
|
837
|
+
* @!method rotation
|
838
|
+
* The rotation part of this transformation
|
839
|
+
* @return [Quaternion]
|
840
|
+
* @!method concatenate(is)
|
841
|
+
* Concatenate self and another transformation
|
842
|
+
* @param [Isometry3] is
|
843
|
+
* @return [Isometry3]
|
844
|
+
* @!method translate(v)
|
845
|
+
* Add a new translation after the rotation
|
846
|
+
* @param [Vector3] v the translation
|
847
|
+
* @return [void]
|
848
|
+
* @!method rotate(q)
|
849
|
+
* Add a new rotation after the translation
|
850
|
+
* @param [Quaternion] q the rotation
|
851
|
+
* @return [void]
|
852
|
+
* @!method pretranslate(v)
|
853
|
+
* Add a new translation before the rotation
|
854
|
+
* @param [Vector3] v the translation
|
855
|
+
* @return [void]
|
856
|
+
* @!method prerotate(q)
|
857
|
+
* Add a new rotation before the translation
|
858
|
+
* @param [Quaternion] q the rotation
|
859
|
+
* @return [void]
|
860
|
+
* @!method matrix
|
861
|
+
* The transformation matrix equivalent to self
|
862
|
+
* @return [MatrixX]
|
863
|
+
*/
|
864
|
+
struct Isometry3
|
865
|
+
{
|
866
|
+
Isometry3d *t;
|
867
|
+
|
868
|
+
Isometry3() : t(new Isometry3d()) { t->setIdentity(); }
|
869
|
+
Isometry3(const Isometry3& _m) : t(new Isometry3d(*_m.t)) {}
|
870
|
+
Isometry3(const Isometry3d& _m) : t(new Isometry3d(_m)) {}
|
871
|
+
~Isometry3() { delete t; }
|
872
|
+
|
873
|
+
Isometry3* inverse() const
|
874
|
+
{ return new Isometry3( t->inverse() ); }
|
875
|
+
|
876
|
+
Vector3* translation() const
|
877
|
+
{ return new Vector3( t->translation() ); }
|
878
|
+
|
879
|
+
Quaternion* rotation() const
|
880
|
+
{ return new Quaternion( Eigen::Quaterniond(t->linear()) ); }
|
881
|
+
|
882
|
+
Isometry3* concatenate(Isometry3 const& other) const
|
883
|
+
{ return new Isometry3( *t * *other.t ); }
|
884
|
+
|
885
|
+
Vector3* transform(Vector3 const& other) const
|
886
|
+
{ return new Vector3( *t * *other.v ); }
|
887
|
+
|
888
|
+
MatrixX* matrix() const
|
889
|
+
{ return new MatrixX( t->matrix() ); }
|
890
|
+
|
891
|
+
void translate( Vector3 const& other ) const
|
892
|
+
{ t->translate( *other.v ); }
|
893
|
+
|
894
|
+
void pretranslate( Vector3 const& other ) const
|
895
|
+
{ t->pretranslate( *other.v ); }
|
896
|
+
|
897
|
+
void rotate( Quaternion const& other ) const
|
898
|
+
{ t->rotate( *other.q ); }
|
899
|
+
|
900
|
+
void prerotate( Quaternion const& other ) const
|
901
|
+
{ t->prerotate( *other.q ); }
|
902
|
+
|
903
|
+
bool operator ==(Isometry3 const& other) const
|
904
|
+
{ return (*this->t).matrix() == (*other.t).matrix(); }
|
905
|
+
|
906
|
+
bool isApprox(Isometry3 const& other, double tolerance)
|
907
|
+
{ return t->isApprox(*other.t, tolerance); }
|
908
|
+
};
|
909
|
+
|
910
|
+
|
911
|
+
/*
|
912
|
+
* Document-class: Eigen::Affine3
|
913
|
+
*
|
914
|
+
* An affine transformation
|
915
|
+
*
|
916
|
+
* @!method approx?(v, threshold = dummy_precision)
|
917
|
+
* Verifies that two transformations are within threshold of each other, elementwise
|
918
|
+
* @param [Isometry3]
|
919
|
+
* @return [Boolean]
|
920
|
+
* @!method inverse
|
921
|
+
* The inverse transformation
|
922
|
+
* @return [Affine3]
|
923
|
+
* @!method translation
|
924
|
+
* The translation part of this transformation
|
925
|
+
* @return [Vector3]
|
926
|
+
* @!method rotation
|
927
|
+
* The rotation part of this transformation
|
928
|
+
* @return [Quaternion]
|
929
|
+
* @!method concatenate(is)
|
930
|
+
* Concatenate self and another transformation
|
931
|
+
* @param [Affine3] is
|
932
|
+
* @return [Affine3]
|
933
|
+
* @!method translate(v)
|
934
|
+
* Add a new translation after the rotation
|
935
|
+
* @param [Vector3] v the translation
|
936
|
+
* @return [void]
|
937
|
+
* @!method rotate(q)
|
938
|
+
* Add a new rotation after the translation
|
939
|
+
* @param [Quaternion] q the rotation
|
940
|
+
* @return [void]
|
941
|
+
* @!method pretranslate(v)
|
942
|
+
* Add a new translation before the rotation
|
943
|
+
* @param [Vector3] v the translation
|
944
|
+
* @return [void]
|
945
|
+
* @!method prerotate(q)
|
946
|
+
* Add a new rotation before the translation
|
947
|
+
* @param [Quaternion] q the rotation
|
948
|
+
* @return [void]
|
949
|
+
* @!method matrix
|
950
|
+
* The transformation matrix equivalent to self
|
951
|
+
* @return [MatrixX]
|
952
|
+
*/
|
953
|
+
struct Affine3
|
954
|
+
{
|
955
|
+
Affine3d *t;
|
956
|
+
|
957
|
+
Affine3() : t(new Affine3d()) { t->setIdentity(); }
|
958
|
+
Affine3(const Affine3& _m) : t(new Affine3d(*_m.t)) {}
|
959
|
+
Affine3(const Affine3d& _m) : t(new Affine3d(_m)) {}
|
960
|
+
~Affine3() { delete t; }
|
961
|
+
|
962
|
+
Affine3* inverse() const
|
963
|
+
{ return new Affine3( t->inverse() ); }
|
964
|
+
|
965
|
+
Vector3* translation() const
|
966
|
+
{ return new Vector3( t->translation() ); }
|
967
|
+
|
968
|
+
Quaternion* rotation() const
|
969
|
+
{ return new Quaternion( Eigen::Quaterniond(t->linear()) ); }
|
970
|
+
|
971
|
+
Affine3* concatenate(Affine3 const& other) const
|
972
|
+
{ return new Affine3( *t * *other.t ); }
|
973
|
+
|
974
|
+
Vector3* transform(Vector3 const& other) const
|
975
|
+
{ return new Vector3( *t * *other.v ); }
|
976
|
+
|
977
|
+
MatrixX* matrix() const
|
978
|
+
{ return new MatrixX( t->matrix() ); }
|
979
|
+
|
980
|
+
void translate( Vector3 const& other ) const
|
981
|
+
{ t->translate( *other.v ); }
|
982
|
+
|
983
|
+
void pretranslate( Vector3 const& other ) const
|
984
|
+
{ t->pretranslate( *other.v ); }
|
985
|
+
|
986
|
+
void rotate( Quaternion const& other ) const
|
987
|
+
{ t->rotate( *other.q ); }
|
988
|
+
|
989
|
+
void prerotate( Quaternion const& other ) const
|
990
|
+
{ t->prerotate( *other.q ); }
|
991
|
+
|
992
|
+
bool operator ==(Affine3 const& other) const
|
993
|
+
{ return (*this->t).matrix() == (*other.t).matrix(); }
|
994
|
+
|
995
|
+
bool isApprox(Affine3 const& other, double tolerance)
|
996
|
+
{ return t->isApprox(*other.t, tolerance); }
|
997
|
+
};
|
998
|
+
|
999
|
+
|
1000
|
+
extern "C" void Init_eigen()
|
1001
|
+
{
|
1002
|
+
Rice::Module rb_mEigen = define_module("Eigen");
|
1003
|
+
|
1004
|
+
Data_Type<Vector3> rb_Vector3 = define_class_under<Vector3>(rb_mEigen, "Vector3")
|
1005
|
+
.define_constructor(Constructor<Vector3,double,double,double>(),
|
1006
|
+
(Arg("x") = static_cast<double>(0),
|
1007
|
+
Arg("y") = static_cast<double>(0),
|
1008
|
+
Arg("z") = static_cast<double>(0)))
|
1009
|
+
.define_method("__equal__", &Vector3::operator ==)
|
1010
|
+
.define_method("norm", &Vector3::norm)
|
1011
|
+
.define_method("normalize!", &Vector3::normalizeBang)
|
1012
|
+
.define_method("normalize", &Vector3::normalize)
|
1013
|
+
.define_method("[]", &Vector3::get)
|
1014
|
+
.define_method("[]=", &Vector3::set)
|
1015
|
+
.define_method("x", &Vector3::x)
|
1016
|
+
.define_method("y", &Vector3::y)
|
1017
|
+
.define_method("z", &Vector3::z)
|
1018
|
+
.define_method("x=", &Vector3::setX)
|
1019
|
+
.define_method("y=", &Vector3::setY)
|
1020
|
+
.define_method("z=", &Vector3::setZ)
|
1021
|
+
.define_method("+", &Vector3::operator +)
|
1022
|
+
.define_method("-", &Vector3::operator -)
|
1023
|
+
.define_method("/", &Vector3::operator /)
|
1024
|
+
.define_method("-@", &Vector3::negate)
|
1025
|
+
.define_method("*", &Vector3::scale)
|
1026
|
+
.define_method("cross", &Vector3::cross)
|
1027
|
+
.define_method("dot", &Vector3::dot)
|
1028
|
+
.define_method("approx?", &Vector3::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));
|
1029
|
+
|
1030
|
+
Data_Type<Quaternion> rb_Quaternion = define_class_under<Quaternion>(rb_mEigen, "Quaternion")
|
1031
|
+
.define_constructor(Constructor<Quaternion,double,double,double,double>())
|
1032
|
+
.define_method("__equal__", &Quaternion::operator ==)
|
1033
|
+
.define_method("w", &Quaternion::w)
|
1034
|
+
.define_method("x", &Quaternion::x)
|
1035
|
+
.define_method("y", &Quaternion::y)
|
1036
|
+
.define_method("z", &Quaternion::z)
|
1037
|
+
.define_method("w=", &Quaternion::setW)
|
1038
|
+
.define_method("x=", &Quaternion::setX)
|
1039
|
+
.define_method("y=", &Quaternion::setY)
|
1040
|
+
.define_method("z=", &Quaternion::setZ)
|
1041
|
+
.define_method("norm", &Quaternion::norm)
|
1042
|
+
.define_method("concatenate", &Quaternion::concatenate)
|
1043
|
+
.define_method("inverse", &Quaternion::inverse)
|
1044
|
+
.define_method("transform", &Quaternion::transform)
|
1045
|
+
.define_method("matrix", &Quaternion::matrix)
|
1046
|
+
.define_method("normalize!", &Quaternion::normalizeBang)
|
1047
|
+
.define_method("normalize", &Quaternion::normalize)
|
1048
|
+
.define_method("approx?", &Quaternion::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
|
1049
|
+
.define_method("to_euler", &Quaternion::toEuler)
|
1050
|
+
.define_method("from_euler", &Quaternion::fromEuler)
|
1051
|
+
.define_method("from_angle_axis", &Quaternion::fromAngleAxis)
|
1052
|
+
.define_method("from_matrix", &Quaternion::fromMatrix);
|
1053
|
+
|
1054
|
+
Data_Type<AngleAxis> rb_AngleAxis = define_class_under<AngleAxis>(rb_mEigen, "AngleAxis")
|
1055
|
+
.define_constructor(Constructor<AngleAxis,double,Vector3 const&>())
|
1056
|
+
.define_method("__equal__", &AngleAxis::operator ==)
|
1057
|
+
.define_method("angle", &AngleAxis::angle)
|
1058
|
+
.define_method("axis", &AngleAxis::axis)
|
1059
|
+
.define_method("concatenate", &AngleAxis::concatenate)
|
1060
|
+
.define_method("inverse", &AngleAxis::inverse)
|
1061
|
+
.define_method("transform", &AngleAxis::transform)
|
1062
|
+
.define_method("matrix", &AngleAxis::matrix)
|
1063
|
+
.define_method("approx?", &AngleAxis::isApprox, (Arg("q"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
|
1064
|
+
.define_method("to_euler", &AngleAxis::toEuler)
|
1065
|
+
.define_method("from_euler", &AngleAxis::fromEuler)
|
1066
|
+
.define_method("from_quaternion", &AngleAxis::fromQuaternion)
|
1067
|
+
.define_method("from_matrix", &AngleAxis::fromMatrix);
|
1068
|
+
|
1069
|
+
Data_Type<VectorX> rb_VectorX = define_class_under<VectorX>(rb_mEigen, "VectorX")
|
1070
|
+
.define_constructor(Constructor<VectorX,int>(),
|
1071
|
+
(Arg("rows") = static_cast<int>(0)))
|
1072
|
+
.define_method("resize", &VectorX::resize)
|
1073
|
+
.define_method("__equal__", &VectorX::operator ==)
|
1074
|
+
.define_method("norm", &VectorX::norm)
|
1075
|
+
.define_method("normalize!", &VectorX::normalizeBang)
|
1076
|
+
.define_method("normalize", &VectorX::normalize)
|
1077
|
+
.define_method("size", &VectorX::size)
|
1078
|
+
.define_method("[]", &VectorX::get)
|
1079
|
+
.define_method("[]=", &VectorX::set)
|
1080
|
+
.define_method("+", &VectorX::operator +)
|
1081
|
+
.define_method("-", &VectorX::operator -)
|
1082
|
+
.define_method("/", &VectorX::operator /)
|
1083
|
+
.define_method("-@", &VectorX::negate)
|
1084
|
+
.define_method("*", &VectorX::scale)
|
1085
|
+
.define_method("dot", &VectorX::dot)
|
1086
|
+
.define_method("approx?", &VectorX::isApprox, (Arg("v"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));
|
1087
|
+
|
1088
|
+
Data_Type<Matrix4> rb_Matrix4 = define_class_under<Matrix4>(rb_mEigen, "Matrix4")
|
1089
|
+
.define_constructor(Constructor<Matrix4>())
|
1090
|
+
.define_method("__equal__", &Matrix4::operator ==)
|
1091
|
+
.define_method("T", &Matrix4::transpose)
|
1092
|
+
.define_method("norm", &Matrix4::norm)
|
1093
|
+
.define_method("rows", &Matrix4::rows)
|
1094
|
+
.define_method("cols", &Matrix4::cols)
|
1095
|
+
.define_method("size", &Matrix4::size)
|
1096
|
+
.define_method("[]", &Matrix4::get)
|
1097
|
+
.define_method("[]=", &Matrix4::set)
|
1098
|
+
.define_method("+", &Matrix4::operator +)
|
1099
|
+
.define_method("-", &Matrix4::operator -)
|
1100
|
+
.define_method("/", &Matrix4::operator /)
|
1101
|
+
.define_method("-@", &Matrix4::negate)
|
1102
|
+
.define_method("*", &Matrix4::scale)
|
1103
|
+
.define_method("dotM", &Matrix4::dotM)
|
1104
|
+
.define_method("approx?", &Matrix4::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));
|
1105
|
+
|
1106
|
+
rb_mEigen.const_set("ComputeFullU", INT2FIX(Eigen::ComputeFullU));
|
1107
|
+
rb_mEigen.const_set("ComputeThinU", INT2FIX(Eigen::ComputeThinU));
|
1108
|
+
rb_mEigen.const_set("ComputeThinV", INT2FIX(Eigen::ComputeThinV));
|
1109
|
+
|
1110
|
+
Data_Type<JacobiSVD> rb_JacobiSVD = define_class_under<JacobiSVD>(rb_mEigen, "JacobiSVD")
|
1111
|
+
.define_method("solve", &JacobiSVD::solve);
|
1112
|
+
|
1113
|
+
Data_Type<MatrixX> rb_MatrixX = define_class_under<MatrixX>(rb_mEigen, "MatrixX")
|
1114
|
+
.define_constructor(Constructor<MatrixX,int,int>(),
|
1115
|
+
(Arg("rows") = static_cast<int>(0),
|
1116
|
+
Arg("cols") = static_cast<int>(0)))
|
1117
|
+
.define_method("resize", &MatrixX::resize)
|
1118
|
+
.define_method("__equal__", &MatrixX::operator ==)
|
1119
|
+
.define_method("T", &MatrixX::transpose)
|
1120
|
+
.define_method("norm", &MatrixX::norm)
|
1121
|
+
.define_method("rows", &MatrixX::rows)
|
1122
|
+
.define_method("cols", &MatrixX::cols)
|
1123
|
+
.define_method("size", &MatrixX::size)
|
1124
|
+
.define_method("[]", &MatrixX::get)
|
1125
|
+
.define_method("[]=", &MatrixX::set)
|
1126
|
+
.define_method("row", &MatrixX::getRow)
|
1127
|
+
.define_method("setRow", &MatrixX::setRow)
|
1128
|
+
.define_method("col", &MatrixX::getColumn)
|
1129
|
+
.define_method("setCol", &MatrixX::setColumn)
|
1130
|
+
.define_method("+", &MatrixX::operator +)
|
1131
|
+
.define_method("-", &MatrixX::operator -)
|
1132
|
+
.define_method("/", &MatrixX::operator /)
|
1133
|
+
.define_method("-@", &MatrixX::negate)
|
1134
|
+
.define_method("*", &MatrixX::scale)
|
1135
|
+
.define_method("dotV", &MatrixX::dotV)
|
1136
|
+
.define_method("dotM", &MatrixX::dotM)
|
1137
|
+
.define_method("jacobiSvd", &MatrixX::jacobiSvd, (Arg("flags") = 0))
|
1138
|
+
.define_method("approx?", &MatrixX::isApprox, (Arg("m"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()));
|
1139
|
+
|
1140
|
+
Data_Type<Isometry3> rb_Isometry3 = define_class_under<Isometry3>(rb_mEigen, "Isometry3")
|
1141
|
+
.define_constructor(Constructor<Isometry3>())
|
1142
|
+
.define_method("__equal__", &Isometry3::operator ==)
|
1143
|
+
.define_method("approx?", &Isometry3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
|
1144
|
+
.define_method("inverse", &Isometry3::inverse)
|
1145
|
+
.define_method("translation", &Isometry3::translation)
|
1146
|
+
.define_method("rotation", &Isometry3::rotation)
|
1147
|
+
.define_method("concatenate", &Isometry3::concatenate)
|
1148
|
+
.define_method("transform", &Isometry3::transform)
|
1149
|
+
.define_method("matrix", &Isometry3::matrix)
|
1150
|
+
.define_method("translate", &Isometry3::translate)
|
1151
|
+
.define_method("pretranslate", &Isometry3::pretranslate)
|
1152
|
+
.define_method("rotate", &Isometry3::rotate)
|
1153
|
+
.define_method("prerotate", &Isometry3::prerotate);
|
1154
|
+
|
1155
|
+
Data_Type<Affine3> rb_Affine3 = define_class_under<Affine3>(rb_mEigen, "Affine3")
|
1156
|
+
.define_constructor(Constructor<Affine3>())
|
1157
|
+
.define_method("__equal__", &Affine3::operator ==)
|
1158
|
+
.define_method("approx?", &Affine3::isApprox, (Arg("i"), Arg("tolerance") = Eigen::NumTraits<double>::dummy_precision()))
|
1159
|
+
.define_method("inverse", &Affine3::inverse)
|
1160
|
+
.define_method("translation", &Affine3::translation)
|
1161
|
+
.define_method("rotation", &Affine3::rotation)
|
1162
|
+
.define_method("concatenate", &Affine3::concatenate)
|
1163
|
+
.define_method("transform", &Affine3::transform)
|
1164
|
+
.define_method("matrix", &Affine3::matrix)
|
1165
|
+
.define_method("translate", &Affine3::translate)
|
1166
|
+
.define_method("pretranslate", &Affine3::pretranslate)
|
1167
|
+
.define_method("rotate", &Affine3::rotate)
|
1168
|
+
.define_method("prerotate", &Affine3::prerotate);
|
1169
|
+
}
|
1170
|
+
|