@@ -120,345 +120,4 @@ end
120
120
@test expected. origin ≈ result. origin
121
121
@test expected. direction ≈ result. direction
122
122
end
123
-
124
- #=
125
- @testset "Rotation2D on Polar" begin
126
- p = Polar(2.0, 1.0)
127
- trans = Rotation2D(1.0)
128
-
129
- # Inverse
130
- @test inv(trans) == Rotation2D(-1.0)
131
-
132
- # Composition
133
- @test trans ∘ trans == Rotation2D(2.0)
134
-
135
- # Transform
136
- @test trans(p) == Polar(2.0, 2.0)
137
-
138
- # Transform derivative
139
- @test transform_deriv(trans, p) == [0 0; 0 1]
140
-
141
- # Transform parameter derivative
142
- @test transform_deriv_params(trans, p) == [0; 1]
143
- end
144
-
145
- @testset "Rotation2D" begin
146
- x = SVector(2.0, 0.0)
147
- x2 = CartesianFromPolar()(Polar(2.0, 1.0))
148
- trans = Rotation2D(1.0)
149
-
150
- # Constructor
151
- @test trans.cos == cos(1.0)
152
- @test trans.sin == sin(1.0)
153
-
154
- # Inverse
155
- @test inv(trans) == Rotation2D(-1.0)
156
-
157
- # Composition
158
- @test trans ∘ trans == Rotation2D(2.0)
159
-
160
- # Transform
161
- @test trans(x) ≈ x2
162
- @test SVector(trans(Tuple(x))) ≈ x2
163
- @test trans(collect(x)) ≈ collect(x2)
164
-
165
- # Transform derivative
166
- x = SVector(2.0,1.0)
167
- x_gn = SVector(Dual(2.0, (1.0,0.0)), Dual(1.0, (0.0,1.0)))
168
- x2_gn = trans(x_gn)
169
- m_gn = @SMatrix [partials(x2_gn[1], 1) partials(x2_gn[1], 2);
170
- partials(x2_gn[2], 1) partials(x2_gn[2], 2) ]
171
- m = transform_deriv(trans, x)
172
- @test m ≈ m_gn
173
-
174
- # Transform parameter derivative
175
- trans_gn = Rotation2D(Dual(1.0, (1.0)))
176
- x = SVector(2.0,1.0)
177
- x2_gn = trans_gn(x)
178
- m_gn = Mat(partials(x2_gn[1], 1), partials(x2_gn[2], 1))
179
- m = transform_deriv_params(trans, x)
180
- @test m ≈ m_gn
181
- end
182
-
183
- @testset "Rotation (3D)" begin
184
-
185
- @testset "Rotation{Void} (rotation matrix parameterization)" begin
186
- θx = 0.1
187
- θy = 0.2
188
- θz = 0.3
189
-
190
- sx = sin(θx)
191
- cx = cos(θx)
192
- sy = sin(θy)
193
- cy = cos(θy)
194
- sz = sin(θz)
195
- cz = cos(θz)
196
-
197
- Rx = [ cx sx 0;
198
- -sx cx 0;
199
- 0 0 1]
200
-
201
- Ry = [1 0 0 ;
202
- 0 cy -sy;
203
- 0 sy cy]
204
-
205
- Rz = [cx 0 -sx;
206
- 0 1 0 ;
207
- sx 0 cx]
208
-
209
- R = Mat{3,3,Float64}(Rx*Ry*Rz)
210
-
211
- x = SVector(1.0, 2.0, 3.0)
212
- trans = Rotation(R)
213
-
214
- @test inv(trans).matrix ≈ R'
215
- @test (trans ∘ trans).matrix ≈ R*R
216
-
217
- y = trans(x)
218
- @test y == R * SVector(1.0, 2.0, 3.0)
219
- @test trans(Tuple(x)) == Tuple(R * SVector(1.0, 2.0, 3.0))
220
- @test trans(collect(x)) == R * SVector(1.0, 2.0, 3.0)
221
-
222
-
223
- x_gn = SVector(Dual(1.0,(1.,0.,0.)), Dual(2.0,(0.,1.,0.)), Dual(3.0,(0.,0.,1.)))
224
- y_gn = trans(x_gn)
225
- M_gn = Mat{3,3,Float64}(vcat(ntuple(i->[partials(y_gn[i], j) for j = 1:3].', 3)...))
226
- M = transform_deriv(trans, x)
227
- @test M ≈ M_gn
228
-
229
- g11 = Dual(R[1,1],(1.,0.,0.,0.,0.,0.,0.,0.,0.))
230
- g12 = Dual(R[1,2],(0.,1.,0.,0.,0.,0.,0.,0.,0.))
231
- g13 = Dual(R[1,3],(0.,0.,1.,0.,0.,0.,0.,0.,0.))
232
- g21 = Dual(R[2,1],(0.,0.,0.,1.,0.,0.,0.,0.,0.))
233
- g22 = Dual(R[2,2],(0.,0.,0.,0.,1.,0.,0.,0.,0.))
234
- g23 = Dual(R[2,3],(0.,0.,0.,0.,0.,1.,0.,0.,0.))
235
- g31 = Dual(R[3,1],(0.,0.,0.,0.,0.,0.,1.,0.,0.))
236
- g32 = Dual(R[3,2],(0.,0.,0.,0.,0.,0.,0.,1.,0.))
237
- g33 = Dual(R[3,3],(0.,0.,0.,0.,0.,0.,0.,0.,1.))
238
-
239
- G = @SMatrix [g11 g12 g13;
240
- g21 g22 g23;
241
- g31 g32 g33 ]
242
-
243
- trans_gn = Rotation(G)
244
- y_gn = trans_gn(x)
245
-
246
- M_gn = Mat{3,9,Float64}(vcat(ntuple(i->[partials(y_gn[i], j) for j = 1:9].', 3)...))
247
- M = transform_deriv_params(trans, x)
248
- @test M ≈ M_gn
249
- end
250
-
251
- @testset "Rotation{Quaternion} (quaternion parameterization)" begin
252
- v = [0.35, 0.45, 0.25, 0.15]
253
- v = v / vecnorm(v)
254
- q = Quaternion(v[1],v[2],v[3],v[4],true)
255
-
256
- trans = Rotation(q)
257
- x = SVector(1.0, 2.0, 3.0)
258
-
259
- @test inv(trans) ≈ Rotation(inv(Quaternion(v[1],v[2],v[3],v[4])))
260
- @test trans ∘ trans ≈ Rotation(trans.matrix * trans.matrix)
261
-
262
- y = trans(x)
263
- @test y ≈ SVector(3.439024390243902,-1.1463414634146332,0.9268292682926829)
264
-
265
- x_gn = SVector(Dual(1.0,(1.,0.,0.)), Dual(2.0,(0.,1.,0.)), Dual(3.0,(0.,0.,1.)))
266
- y_gn = trans(x_gn)
267
- M_gn = Mat{3,3,Float64}(vcat(ntuple(i->[partials(y_gn[i], j) for j = 1:3].', 3)...))
268
- M = transform_deriv(trans, x)
269
- @test M ≈ M_gn
270
-
271
- v_gn = [Dual(v[1],(1.,0.,0.,0.)), Dual(v[2],(0.,1.,0.,0.)), Dual(v[3],(0.,0.,1.,0.)), Dual(v[4],(0.,0.,0.,1.))]
272
- q_gn = Quaternion(v_gn[1],v_gn[2],v_gn[3],v_gn[4],true)
273
- trans_gn = Rotation(q_gn)
274
- y_gn = trans_gn(x)
275
- M_gn = Mat{3,4,Float64}(vcat(ntuple(i->[partials(y_gn[i], j) for j = 1:4].', 3)...))
276
- M = transform_deriv_params(trans, x)
277
- # Project both to tangent plane of normalized quaternions (there seems to be a change in definition...)
278
- proj = Mat{4,4,Float64}(eye(4) - v*v')
279
- @test M*proj ≈ M_gn*proj
280
- end
281
-
282
- @testset "Rotation{EulerAngles} (Euler angle parameterization)" begin
283
- θx = 0.1
284
- θy = 0.2
285
- θz = 0.3
286
-
287
- trans = Rotation(EulerAngles(θx, θy, θz))
288
- x = SVector(1.0, 2.0, 3.0)
289
-
290
- @test inv(trans) == Rotation(trans.matrix')
291
- @test trans ∘ trans == Rotation(trans.matrix * trans.matrix)
292
-
293
- y = trans(x)
294
- @test y == SVector(0.9984766744283545,2.1054173473736495,2.92750100324502)
295
-
296
- x_gn = SVector(Dual(1.0,(1.,0.,0.)), Dual(2.0,(0.,1.,0.)), Dual(3.0,(0.,0.,1.)))
297
- y_gn = trans(x_gn)
298
- M_gn = Mat{3,3,Float64}(vcat(ntuple(i->[partials(y_gn[i], j) for j = 1:3].', 3)...))
299
- M = transform_deriv(trans, x)
300
- @test M ≈ M_gn
301
-
302
- # Parameter derivative not defined
303
- # TODO ?
304
- end
305
-
306
- @testset "RotationXY, RotationYZ and RotationZX" begin
307
- # RotationXY
308
- x = SVector(2.0, 0.0, 0.0)
309
- x2 = CartesianFromSpherical()(Spherical(2.0, 1.0, 0.0))
310
- trans = RotationXY(1.0)
311
-
312
- # Constructor
313
- @test trans.cos == cos(1.0)
314
- @test trans.sin == sin(1.0)
315
-
316
- # Inverse
317
- @test inv(trans) == RotationXY(-1.0)
318
- @test RotationYX(1.0) == RotationXY(-1.0)
319
-
320
- # Composition
321
- @test trans ∘ trans == RotationXY(2.0)
322
-
323
- # Transform
324
- @test trans(x) ≈ x2
325
- @test SVector(trans(Tuple(x))) ≈ x2
326
- @test SVector(trans(collect(x))) ≈ x2
327
-
328
- # Transform derivative
329
- x = SVector(2.0,1.0,3.0)
330
- x_gn = SVector(Dual(2.0, (1.0,0.0,0.0)), Dual(1.0, (0.0,1.0,0.0)), Dual(3.0, (0.0,0.0,1.0)))
331
- x2_gn = trans(x_gn)
332
- m_gn = @SMatrix [partials(x2_gn[1], 1) partials(x2_gn[1], 2) partials(x2_gn[1], 3);
333
- partials(x2_gn[2], 1) partials(x2_gn[2], 2) partials(x2_gn[2], 3);
334
- partials(x2_gn[3], 1) partials(x2_gn[3], 2) partials(x2_gn[3], 3) ]
335
- m = transform_deriv(trans, x)
336
- @test m ≈ m_gn
337
-
338
- # Transform parameter derivative
339
- trans_gn = RotationXY(Dual(1.0, (1.0)))
340
- x = SVector(2.0,1.0,3.0)
341
- x2_gn = trans_gn(x)
342
- m_gn = Mat(partials(x2_gn[1], 1), partials(x2_gn[2], 1), partials(x2_gn[3], 1))
343
- m = transform_deriv_params(trans, x)
344
- @test m ≈ m_gn
345
-
346
-
347
- # RotationYZ
348
- x = SVector(0.0, 2.0, 0.0)
349
- x2 = CartesianFromSpherical()(Spherical(2.0, pi/2, 1.0))
350
- trans = RotationYZ(1.0)
351
-
352
- # Constructor
353
- @test trans.cos == cos(1.0)
354
- @test trans.sin == sin(1.0)
355
-
356
- # Inverse
357
- @test inv(trans) == RotationYZ(-1.0)
358
- @test RotationZY(1.0) == RotationYZ(-1.0)
359
-
360
- # Composition
361
- @test trans ∘ trans == RotationYZ(2.0)
362
-
363
- # Transform
364
- @test trans(x) ≈ x2
365
- @test SVector(trans(Tuple(x))) ≈ x2
366
- @test SVector(trans(collect(x))) ≈ x2
367
-
368
- # Transform derivative
369
- x = SVector(2.0,1.0,3.0)
370
- x_gn = SVector(Dual(2.0, (1.0,0.0,0.0)), Dual(1.0, (0.0,1.0,0.0)), Dual(3.0, (0.0,0.0,1.0)))
371
- x2_gn = trans(x_gn)
372
- m_gn = @SMatrix [partials(x2_gn[1], 1) partials(x2_gn[1], 2) partials(x2_gn[1], 3);
373
- partials(x2_gn[2], 1) partials(x2_gn[2], 2) partials(x2_gn[2], 3);
374
- partials(x2_gn[3], 1) partials(x2_gn[3], 2) partials(x2_gn[3], 3) ]
375
- m = transform_deriv(trans, x)
376
- @test m ≈ m_gn
377
-
378
- # Transform parameter derivative
379
- trans_gn = RotationYZ(Dual(1.0, (1.0)))
380
- x = SVector(2.0,1.0,3.0)
381
- x2_gn = trans_gn(x)
382
- m_gn = Mat(partials(x2_gn[1], 1), partials(x2_gn[2], 1), partials(x2_gn[3], 1))
383
- m = transform_deriv_params(trans, x)
384
- @test m ≈ m_gn
385
-
386
-
387
- # RotationZX
388
- x = SVector(2.0, 0.0, 0.0)
389
- x2 = CartesianFromSpherical()(Spherical(2.0, 0.0, -1.0))
390
- trans = RotationZX(1.0)
391
-
392
- # Constructor
393
- @test trans.cos == cos(1.0)
394
- @test trans.sin == sin(1.0)
395
-
396
- # Inverse
397
- @test inv(trans) == RotationZX(-1.0)
398
- @test RotationXZ(1.0) == RotationZX(-1.0)
399
-
400
- # Composition
401
- @test trans ∘ trans == RotationZX(2.0)
402
-
403
- # Transform
404
- @test trans(x) ≈ x2
405
- @test SVector(trans(Tuple(x))) ≈ x2
406
- @test SVector(trans(collect(x))) ≈ x2
407
-
408
- # Transform derivative
409
- x = SVector(2.0,1.0,3.0)
410
- x_gn = SVector(Dual(2.0, (1.0,0.0,0.0)), Dual(1.0, (0.0,1.0,0.0)), Dual(3.0, (0.0,0.0,1.0)))
411
- x2_gn = trans(x_gn)
412
- m_gn = @SMatrix [partials(x2_gn[1], 1) partials(x2_gn[1], 2) partials(x2_gn[1], 3);
413
- partials(x2_gn[2], 1) partials(x2_gn[2], 2) partials(x2_gn[2], 3);
414
- partials(x2_gn[3], 1) partials(x2_gn[3], 2) partials(x2_gn[3], 3) ]
415
- m = transform_deriv(trans, x)
416
- @test m ≈ m_gn
417
-
418
- # Transform parameter derivative
419
- trans_gn = RotationZX(Dual(1.0, (1.0)))
420
- x = SVector(2.0,1.0,3.0)
421
- x2_gn = trans_gn(x)
422
- m_gn = Mat(partials(x2_gn[1], 1), partials(x2_gn[2], 1), partials(x2_gn[3], 1))
423
- m = transform_deriv_params(trans, x)
424
- @test m ≈ m_gn
425
- end
426
-
427
- @testset "euler_rotation() and composed derivatives" begin
428
- x = SVector(2.0,1.0,3.0)
429
- trans = euler_rotation(0.1,0.2,0.3)
430
- x2 = SVector(2.730537054338937,0.8047190852558106,2.428290466296628)
431
-
432
- #@test trans.t1.t1 == RotationXY(0.1)
433
- #@test trans.t1.t2 == RotationYZ(0.2)
434
- #@test trans.t2 == RotationZX(0.3)
435
-
436
- @test inv(trans) ≈ RotationZX(-0.3) ∘ (RotationYZ(-0.2) ∘ RotationXY(-0.1))
437
-
438
- @test trans(x) ≈ x2
439
-
440
- # Transform derivative
441
- x = SVector(2.0,1.0,3.0)
442
- x_gn = SVector(Dual(2.0, (1.0,0.0,0.0)), Dual(1.0, (0.0,1.0,0.0)), Dual(3.0, (0.0,0.0,1.0)))
443
- x2_gn = trans(x_gn)
444
- m_gn = @SMatrix [partials(x2_gn[1], 1) partials(x2_gn[1], 2) partials(x2_gn[1], 3);
445
- partials(x2_gn[2], 1) partials(x2_gn[2], 2) partials(x2_gn[2], 3);
446
- partials(x2_gn[3], 1) partials(x2_gn[3], 2) partials(x2_gn[3], 3) ]
447
- m = transform_deriv(trans, x)
448
- @test m ≈ m_gn
449
-
450
- # Transform parameter derivative
451
-
452
- #trans_gn = euler_rotation(Dual(0.1, (1.0, 0.0, 0.0)), Dual(0.2, (0.0, 1.0, 0.0)), Dual(0.3, (0.0, 0.0, 1.0)))
453
- #x = SVector(2.0,1.0,3.0)
454
- #x2_gn = trans_gn(x)
455
- #m_gn = @SMatrix [partials(x2_gn[1], 1) partials(x2_gn[1], 2) partials(x2_gn[1], 3);
456
- # partials(x2_gn[2], 1) partials(x2_gn[2], 2) partials(x2_gn[2], 3);
457
- # partials(x2_gn[3], 1) partials(x2_gn[3], 2) partials(x2_gn[3], 3)]
458
- #m = transform_deriv_params(trans, x)
459
- #@test m ≈ m_gn
460
-
461
- end
462
- end
463
- =#
464
123
end
0 commit comments