old_contents: from __future__ import print_function
  
  from eigenpy import *
  from geometry import *
  import numpy as np
  from numpy import cos,sin
  
  verbose = True
  
  def isapprox(a,b,epsilon=1e-6):
      if issubclass(a.__class__,np.ndarray) and issubclass(b.__class__,np.ndarray):
          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)
  assert(q==q)
  assert(isapprox(q.coeffs(),q2.coeffs()))
  assert(q2.isApprox(q2))
  assert(q2.isApprox(q2,1e-2))
  
  Rq = q.matrix()
  Rr = r.matrix()
  assert(isapprox(Rq*Rq.T,np.eye(3)))
  assert(isapprox(Rr,Rq))
  
  qR = Quaternion(Rr)
  assert(q.isApprox(qR))
  assert(isapprox(q.coeffs(),qR.coeffs()))
  
  assert(isapprox(qR[3],1./np.sqrt(30)))
  try:
     qR[5]
     print("Error, this message should not appear.")
  except RuntimeError as e:
    if verbose: print("As expected, catched exception: ",e)
  
  # --- Angle Vector ------------------------------------------------
  r = AngleAxis(.1,np.matrix([1,0,0],np.double).T)
  if verbose: print("Rx(.1) = \n\n",r.matrix(),"\n")
  assert( isapprox(r.matrix()[2,2],cos(r.angle)))
  assert( isapprox(r.axis,np.matrix("1;0;0")) )
  assert( isapprox(r.angle,0.1) )
  assert(r.isApprox(r))
  assert(r.isApprox(r,1e-2))
  
  r.axis = np.matrix([0,1,0],np.double).T
  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)
  res = testInQuaternion(q)
  assert(q.norm() == res)
  
new_contents: from __future__ import print_function
  
  from geometry import *
  import numpy as np
  from numpy import cos,sin
  
  verbose = True
  
  def isapprox(a,b,epsilon=1e-6):
      if issubclass(a.__class__,np.ndarray) and issubclass(b.__class__,np.ndarray):
          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)
  assert(q==q)
  assert(isapprox(q.coeffs(),q2.coeffs()))
  assert(q2.isApprox(q2))
  assert(q2.isApprox(q2,1e-2))
  
  Rq = q.matrix()
  Rr = r.matrix()
  assert(isapprox(Rq*Rq.T,np.eye(3)))
  assert(isapprox(Rr,Rq))
  
  qR = Quaternion(Rr)
  assert(q.isApprox(qR))
  assert(isapprox(q.coeffs(),qR.coeffs()))
  
  assert(isapprox(qR[3],1./np.sqrt(30)))
  try:
     qR[5]
     print("Error, this message should not appear.")
  except RuntimeError as e:
    if verbose: print("As expected, catched exception: ",e)
  
  # --- Angle Vector ------------------------------------------------
  r = AngleAxis(.1,np.matrix([1,0,0],np.double).T)
  if verbose: print("Rx(.1) = \n\n",r.matrix(),"\n")
  assert( isapprox(r.matrix()[2,2],cos(r.angle)))
  assert( isapprox(r.axis,np.matrix("1;0;0")) )
  assert( isapprox(r.angle,0.1) )
  assert(r.isApprox(r))
  assert(r.isApprox(r,1e-2))
  
  r.axis = np.matrix([0,1,0],np.double).T
  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)
  res = testInQuaternion(q)
  assert(q.norm() == res)
  
subject: fix test