eigen 0.1.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- 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
|
+
[](https://travis-ci.org/rock-core/base-ruby_eigen)
|
4
|
+
[](http://badge.fury.io/rb/eigen)
|
5
|
+
[](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
|
+
|