test_geometry.py 1.77 KB
Newer Older
1
2
from __future__ import print_function

Nicolas Mansard's avatar
Nicolas Mansard committed
3
4
5
6
from geometry import *
import numpy as np
from numpy import cos,sin

Guilhem Saurel's avatar
Guilhem Saurel committed
7
verbose = True
Nicolas Mansard's avatar
Nicolas Mansard committed
8
9

def isapprox(a,b,epsilon=1e-6):
10
    if issubclass(a.__class__,np.ndarray) and issubclass(b.__class__,np.ndarray):
Nicolas Mansard's avatar
Nicolas Mansard committed
11
12
13
14
15
16
17
18
19
20
21
22
        return np.allclose(a,b,epsilon)
    else:
        return abs(a-b)<epsilon

# --- Quaternion ---------------------------------------------------------------
q = Quaternion(1,2,3,4)
q.normalize()
assert(isapprox(np.linalg.norm(q.coeffs()),q.norm()))
assert(isapprox(np.linalg.norm(q.coeffs()),1))

r = AngleAxis(q)
q2 = Quaternion(r)
Justin Carpentier's avatar
Justin Carpentier committed
23
assert(q==q)
Nicolas Mansard's avatar
Nicolas Mansard committed
24
25
26
27
assert(isapprox(q.coeffs(),q2.coeffs()))

Rq = q.matrix()
Rr = r.matrix()
28
assert(isapprox(Rq*Rq.T,np.eye(3)))
Nicolas Mansard's avatar
Nicolas Mansard committed
29
30
31
assert(isapprox(Rr,Rq))

qR = Quaternion(Rr)
Justin Carpentier's avatar
Justin Carpentier committed
32
assert(q.isApprox(qR))
Nicolas Mansard's avatar
Nicolas Mansard committed
33
34
35
36
37
assert(isapprox(q.coeffs(),qR.coeffs()))

assert(isapprox(qR[3],1./np.sqrt(30)))
try:
   qR[5]
38
   print("Error, this message should not appear.")
Guilhem Saurel's avatar
Guilhem Saurel committed
39
40
except RuntimeError as e:
  if verbose: print("As expected, catched exception: ",e)
Nicolas Mansard's avatar
Nicolas Mansard committed
41
42

# --- Angle Vector ------------------------------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
43
r = AngleAxis(.1,np.matrix([1,0,0],np.double).T)
44
if verbose: print("Rx(.1) = \n\n",r.matrix(),"\n")
Nicolas Mansard's avatar
Nicolas Mansard committed
45
assert( isapprox(r.matrix()[2,2],cos(r.angle)))
46
assert( isapprox(r.axis,np.matrix("1;0;0")) )
Nicolas Mansard's avatar
Nicolas Mansard committed
47
48
assert( isapprox(r.angle,0.1) )

Guilhem Saurel's avatar
Guilhem Saurel committed
49
r.axis = np.matrix([0,1,0],np.double).T
Nicolas Mansard's avatar
Nicolas Mansard committed
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
assert( isapprox(r.matrix()[0,0],cos(r.angle)))

ri = r.inverse()
assert( isapprox(ri.angle,-.1) )

R = r.matrix()
r2 = AngleAxis(np.dot(R,R))
assert( isapprox(r2.angle,r.angle*2) )

# --- USER FUNCTIONS -----------------------------------------------------------
ro = testOutAngleAxis()
assert(ro.__class__ == AngleAxis)
res = testInAngleAxis(r)
assert( res==r.angle )

qo = testOutQuaternion()
assert(qo.__class__ == Quaternion)
Guilhem Saurel's avatar
Guilhem Saurel committed
67
res = testInQuaternion(q)
Nicolas Mansard's avatar
Nicolas Mansard committed
68
assert(q.norm() == res)