OpenOffice.org Forum at OOoForum.orgThe OpenOffice.org Forum
 
 [Home]   [FAQ]   [Search]   [Memberlist]   [Usergroups]   [Register
 [Profile]   [Log in to check your private messages]   [Log in

ooCalc as a tool for x3d and vrml rotations with quaternions

 
Post new topic   Reply to topic    OOoForum.org Forum Index -> OpenOffice.org Code Snippets
View previous topic :: View next topic  
Author Message
bertram
Power User
Power User


Joined: 13 Nov 2005
Posts: 55

PostPosted: Sun Sep 03, 2006 2:44 pm    Post subject: ooCalc as a tool for x3d and vrml rotations with quaternions Reply with quote

Code:
REM  *****  BASIC  *****

Option Explicit

Function SphaericPlacement( Px, Py, Pz, Qx, Qy, Qz as Double ) as String

'This macro demonstrates a use of ooCalc for hard coding 3D models.

'Given two points P and Q on the surface of a sphere centered on O, and an
'asymmetric object OBJ extruded on the y-axis and oriented "up" from -1 to
'1, SphaericPlacement macro writes the transform statement(s) required to
'rescale, translate and rotate the object in 3d, such that (A) the -1 to +1
'y-axis endpoints are moved to points P and Q respectively, and (B) the
'x-axis of its asymmetric xz-cross section (its "look-at" direction) is
'radially outward from the sphere.

'ooCalc can be used to establish coordinates from mathematical relations,
'such that objects can be computed and placed (the typical sequence in
'scientific modeling, with its emphasis on coordinate control rather than
'on object design). The macro computes and writes the required fields for
'reducing vrml axis-angle rotations and transforms ... reducing three
'rotations down to one -- cleaning up vrml/x3d code space and decreasing
'load time. It could easily be converted to javascript using vrml/x3d
'maths and object types.

'Quaternions are used to merge axis-angle rotations.

'Several vrml models of solids were built to proof the "corner spots" in the
'model, and the transforms worked. My notes follow vrml conventions: +x-axis
'is to the right, +y-axis is up, and +z-axis is towards viewer.  I'm not a
'programmer, so there are improvements to be made.  A nice summary of 3D-
'quaternion maths was found at www.j3d.org/matrix_faq/matrfaq_latest.html.

'revised to fix chunkiness of sqr(2) irrationals when inversing transcendentals
'around line 446

   Const pi as Double = 3.14159265358979
   Const twopi as Double = 6.28318530717959
   Const halfpi as Double = 1.57079632679490
   Const fStr as String = "+0.000000 ;-0.000000 "

   Dim nonzero as Integer
   Dim arg1( 1 to 1 ) as Double
   Dim args2( 1 to 2 ) as Double
   Dim oFunctionAccess
   Dim errorCase as Integer
   Dim errorMsg( 0 to 7 ) as String

   Dim Pr, Qr as Double
   Dim Rx, Ry, Rz, Rr as Double
   Dim Tx, Ty, Tz, Tr, Ts as Double
   Dim scale as Double
   Dim Ux, Uy, Uz, Ur as Double
   Dim UTx, UTy, UTz as Double
   Dim UdotR as Double
   Dim UcrRx, UcrRy, UcrRz, UcrRr as Double
   Dim A, B, C, D, E as Double
   Dim sinA, cosA, sinB, cosB as Double
   Dim R1x, R1y, R1z, R1a, R1r as Double
   Dim R2x, R2y, R2z, R2a, R2r as Double
   Dim R3x, R3y, R3z, R3a, R3r as Double
   Dim Q1x, Q1y, Q1z, Q1w as Double
   Dim Q2x, Q2y, Q2z, Q2w as Double
   Dim Q3x, Q3y, Q3z, Q3w as Double
   Dim Qix, Qiy, Qiz, Qiw as Double
   Dim Qrx, Qry, Qrz, Qrw as Double
   Dim Rox, Roy, Roz, Roa as Double
   Dim rStr as String

   'on error goto ErrorHandler

   'initialize internals
   SphaericPlacement = ""
   oFunctionAccess = createUnoService( "com.sun.star.sheet.FunctionAccess" )

'dim Px, Py, Pz, Qx, Qy, Qz as Double
'dim SphaericPlacement as String
'Px = 0 : Py= sqr(2.)/2. : Pz = 0. : Qx= sqr(2.)/2. : Qy= 0. : Qz= 0.
'Px = sqr(2.)/2. : Py= 0. : Pz = 0. : Qx= 0. : Qy= 0. : Qz= -sqr(2.)/2.

   'setup an error table
   errorMsg(0) = "either trigonometry is broken or ... I'm clueless!"
   errorMsg(1) = "error returned: target sphere has no radius ( P = 0 )."
   errorMsg(2) = "error returned: target sphere is not round ( Pr <> Qr )."
   errorMsg(3) = "error returned: target vector has no length ( P = Q )."
   errorMsg(4) = "error returned: insufficient parameters to orient a diameter."
   errorMsg(5) = "error returned: a rotation vector has its length missing."
   errorMsg(6) = "one or more angles is greater than 2*pi..."
   errorMsg(7) = "one or more angles is less than -pi..."

   'setup a coordinate system
   'P and Q are given as any two points on a sphere not diagonal to each other
   '(Asymmetric diagonals require an additional parameter to be fully located.)
   'R is a directional chord from point P to point Q.  M is midpoint of R.
   'T is a translation vector from O to M, thus Tx = Mx, ...
   'Ts is a 2d-vector for the "shadow" of T on xz-plane
   Tx = ( Px+Qx )/2. : Ty = (Py+Qy)/2. : Tz = (Pz+Qz)/2.
   Tr = sqr( Tx*Tx + Ty*Ty + Tz*Tz )
   Ts = sqr( Tx*Tx + Tz*Tz )
   'compute and normalize target vector R = Q - P ( target "up" is from P to Q )
   Rx = Qx - Px : Ry = Qy - Py : Rz = Qz - Pz
   Rr = sqr(Rx*Rx + Ry*Ry + Rz*Rz)
   'scale factor of the transform is R/2, since object is extruded from y=-1 to y=1
   scale = Rr / 2.
   
   'screen input data and setup an errorbunch for basic rubbish
   Pr = sqr( Px*Px + Py*Py + Pz*Pz )
   Qr = sqr( Qx*Qx + Qy*Qy + Qz*Qz )
   if Pr = 0. or Qr = 0. then 'target sphere has no radius
      errorCase = 1 : goto errorHandler
   elseIf abs( (Qr - Pr) / (Qr + Pr) ) > 0.000001 then 'target sphere is not round
      errorCase = 2 : goto errorHandler
   elseIf Rr = 0. then 'target chord has no length
      errorCase = 3 : goto errorHandler
   elseIf Tr = 0 then 'target chord is a diagonal = yet another proggie
      errorCase = 4 : goto errorHandler
   end if

   'compute axis-angle rotation sets R1, R2 and R3 to set orientation
   'solution depends on geometry of Ts
   if Ts <> 0 then
      'vector T has a shadow on the xz-plane, and M is not on the y-axis
      'define unit "up-vector" Uo as [0 1 0] with "look-at" of [1 0 0]
      '--compute angle A (-pi to pi) of Ts in the xz-plane as a "horizon E-azimuth"
      'axis of A is +y-axis, turning +ccw from +x-axis, +A -> -z
      cosA = Tx / Ts : sinA = -Tz / Ts
      args2(1) = cosA : args2(2) = sinA
      A = oFunctionAccess.callFunction( "atan2", args2() )
      'R3-rotation summary (first in geometry, third in computation):
      R3x = 0. : R3y = 1. : R3z = 0. : R3a = A
      '--compute angle B (-pi/2 to pi/2), as "elevation" above xz-plane
      'axis of B is [ sinA 0 cosA ] (perpendicular to Ts in xz-plane), +B -> +y
      cosB = Ts / Tr : sinB = Ty / Tr
      arg1(1) = sinB
      B = oFunctionAccess.callFunction( "asin", arg1() )
      'B-rotation summary
      R2x = sinA : R2y = 0. : R2z = cosA : R2a = B
      '--compute angle C (0 to pi) to rotate object (after A and B) to R
      'axis of C is co-linear with T - but still need direction
      'locate the up-vector U after the object is "tilted back" by rotations A and B
      Ux = -sinB * cosA : Uy = cosB : Uz = sinB * sinA
      Ur = sqr( Ux*Ux + Uy*Uy + Uz*Uz )
      'absent transcendental chunkiness, U would be normalized by geometry ...
      'normalize U and R and compute dot-product UdotR and rotation C
      Ux = Ux / Ur : Uy = Uy / Ur : Uz = Uz / Ur
      Rx = Rx / Rr : Ry = Ry / Rr : Rz = Rz / Rr
      UdotR = Ux*Rx + Uy*Ry + Uz*Rz
      'cleanup some irrational <> transcendental chunkiness near C = 0 and 2pi
      if UdotR > 0.9999999999 then UdotR = 1
      If UdotR < -0.9999999999 then UdotR = -1
      arg1(1) = UdotR
      C = oFunctionAccess.callFunction( "acos", arg1() )
      'use cross product UcrR of unit vectors U and R as C axis
      UcrRx = Uy * Rz - Ry * Uz
      UcrRy = Uz * Rx - Rz * Ux
      UcrRz = Ux * Ry - Rx * Uy
      UcrRr = sqr( UcrRx*UcrRx + UcrRy*UcrRy + UcrRz*UcrRz )
      'normalize UcrR and summarize rotation set R1
      if UcrRr <> 0. then
         UcrRx = UcrRx / UcrRr : UcrRy = UcrRy / UcrRr : UcrRz = UcrRz / UcrRr
         R1x = UcrRx : R1y = UcrRy : R1z = UcrRz : R1a = C
      else
         'U cross Rr is zero, and U and R are co-linear
         'direction of C-axis doesn't matter since C = 0 or pi, so use T
         'normalize T to UT (note Tr<>0 and R<>diagonal)
         UTx = Tx / Tr : UTy = Ty / Tr : UTz = Tz / Tr
         R1x = UTx : R1y = UTy : R1z = UTz : R1a = C
      end if
   else
      'M is on the y-axis and T has no shadow on xz-plane
      'note Ty!=0, Ts=0, M=T=[ 0 y 0 ], R=[ x 0 z ], R>0 and T>O
      'rotate unit "up-vector" U to "look at" Ty
      Ux = -Ty / abs( Ty ) ': Uy = 0. : Uz = 0.
      'setup D-rotation on +z-axis to align y-axis extrusion with U
      D = halfpi
      R2x = 0. : R2y = 0. : R2z = -Ux : R2a = D
      'setup E-rotation on +y-axis, rotating U (obj) to R (target)
      'E is 0 to pi, direction of E provided by direction of UcrR
      Rx = Rx / Rr    'normalize Rx
      UdotR = Ux*Rx   'UdotR = Ux*Rx + (Uy=0)*(Ry=0) + (Uz=0)*Rz
      arg1(1) = UdotR
      E = oFunctionAccess.callFunction( "acos", arg1() )
      'UcrRx = 0.     'UcrRx = (Uy=0)* Rz  - (Ry=0)* Uz
      'UcrRy = -Ux*Rz 'UcrRy = (Uz=0)* Rx  -  Rz*Ux
      'UcrRz = 0.     'UcrRz =  Ux *(Ry=0) -  Rx *(Uy=0)
      'if Rz = 0, Rx = 1, E = 0 or pi, and direction doesn't matter
      'normalize UcrR ...
      UcrRy = 1. : if Rz <> 0. then UcrRy = Rz / abs( Rz )
      R1x = 0. : R1y = -Ux*UcrRy : R1z = 0. : R1a = E
      'declare the R3 set as a nullity (zero angle on arbitrary axis)
      R3x = 0. : R3y = 1. : R3z = 0. : R3a = 0.
   end If

   '==consolidate data for separation as a sub==
   'scale is scale
   'translation is [ Tx Ty Tz ]
   'compute rotation C [ UcrRx UcrRy UcrRz C ]
   'then rotation B [ sinA 0 cosA B ]
   'last rotation A [ 0 1 0 A ]

   'screen rotation input and populate crude errorville
   R1r = sqr( R1x*R1x + R1y*R1y + R1z*R1z )
   R2r = sqr( R2x*R2x + R2y*R2y + R2z*R2z )
   R3r = sqr( R3x*R3x + R3y*R3y + R3z*R3z )
   if R1r = 0. or R2r = 0. or R3r = 0. then 'undefined axis of rotation
      errorCase = 5 : goto errorHandler
   'elseif R1a > twopi or R2a > twopi or R3a > twopi then
   '      errorCase = 6 : goto errorHandler
   'elseif R1a < -pi or R2a < -pi or R3a < -pi then
   '   errorCase = 7 : goto errorHandler
   end if
   'reduce angles to range of -2pi to 2pi
   if abs( R3a ) > twopi then realMod( R3a, twopi )
   if abs( R2a ) > twopi then realMod( R2a, twopi )
   if abs( R1a ) > twopi then realMod( R1a, twopi )
   'normalize incoming rotation vectors prior to quaternization
   R1x = R1x / R1r : R1y = R1y / R1r : R1z = R1z / R1r
   R2x = R2x / R2r : R2y = R2y / R2r : R2z = R2z / R2r
   R3x = R3x / R3r : R3y = R3y / R3r : R3z = R3z / R3r
   'determine which rotations have meaningful angles
   nonzero = 0.
      if R1a <> 0. then nonzero = nonzero + 1.
      if R2a <> 0. then nonzero = nonzero + 2.
      if R3a <> 0. then nonzero = nonzero + 4.
   'print nonzero
   select case nonzero
   case 0 'all angles are zero, return a null rotation
      Rox = 1. : Roy = 0. : Roz = 0. : Roa = 0.
   case 1 'the R1-C rotation is only non-zero
      Rox = R1x : Roy = R1y : Roz = R1z : Roa = R1a
   case 2 'the R2-B rotation is the only non-zero         
      Rox = R2x : Roy = R2y : Roz = R2z : Roa = R2a
   case 4 'the R3-A rotation is the only non-zero
      Rox = R3x : Roy = R3y : Roz = R3z : Roa = R3a
   case 3 'the R1-C and R2-B rotations are non-zero
      Rot2Quat( R1x, R1y, R1z, R1a, Q1x, Q1y, Q1z, Q1w )
      Rot2Quat( R2x, R2y, R2z, R2a, Q2x, Q2y, Q2z, Q2w )
      multQuat( Q1x, Q1y, Q1z, Q1w, Q2x, Q2y, Q2z, Q2w, Qrx, Qry, Qrz, Qrw )
      Quat2Rot( Qrx, Qry, Qrz, Qrw, Rox, Roy, Roz, Roa )
   case 5 'the R1-C and R3-A rotations are non-zero
      Rot2Quat( R1x, R1y, R1z, R1a, Q1x, Q1y, Q1z, Q1w )
      Rot2Quat( R3x, R3y, R3z, R3a, Q3x, Q3y, Q3z, Q3w )
      multQuat( Q1x, Q1y, Q1z, Q1w, Q3x, Q3y, Q3z, Q3w, Qrx, Qry, Qrz, Qrw )
      Quat2Rot( Qrx, Qry, Qrz, Qrw, Rox, Roy, Roz, Roa )
   case 6 'the R2-B and R3-A rotations are non-zero
      Rot2Quat( R2x, R2y, R2z, R2a, Q2x, Q2y, Q2z, Q2w )
      Rot2Quat( R3x, R3y, R3z, R3a, Q3x, Q3y, Q3z, Q3w )
      multQuat( Q2x, Q2y, Q2z, Q2w, Q3x, Q3y, Q3z, Q3w, Qrx, Qry, Qrz, Qrw )
      Quat2Rot( Qrx, Qry, Qrz, Qrw, Rox, Roy, Roz, Roa )
   case 7 'all rotations are non-zero
      Rot2Quat( R1x, R1y, R1z, R1a, Q1x, Q1y, Q1z, Q1w )
      Rot2Quat( R2x, R2y, R2z, R2a, Q2x, Q2y, Q2z, Q2w )
      Rot2Quat( R3x, R3y, R3z, R3a, Q3x, Q3y, Q3z, Q3w )
      multQuat( Q1x, Q1y, Q1z, Q1w, Q2x, Q2y, Q2z, Q2w, Qix, Qiy, Qiz, Qiw )
      multQuat( Qix, Qiy, Qiz, Qiw, Q3x, Q3y, Q3z, Q3w, Qrx, Qry, Qrz, Qrw )
      Quat2Rot( Qrx, Qry, Qrz, Qrw, Rox, Roy, Roz, Roa )
   end select

   'the result
   rStr = ""
   rStr = rStr + "rotation " + Format( Rox, fStr ) + Format( Roy, fStr )
   rStr = rStr + Format( Roz, fStr ) + Format( Roa, fStr )
   rStr = rStr + "translation " + Format( Tx, fStr )
   rStr = rStr + Format( Ty, fStr ) + Format( Tz, fStr )
   rStr = rStr + "scale " + Format( scale, fStr ) + Format( scale, fStr )
   rStr = rStr + Format( scale, fStr )
   SphaericPlacement = rStr
   'print SphaericPlacement

   exit Function

   errorHandler:
   select case errorCase
      case 1 : SphaericPlacement = errorMsg(1) : exit Function
      case 2 : SphaericPlacement = errorMsg(2) : exit Function
      case 3 : SphaericPlacement = errorMsg(3) : exit Function
      case 4 : SphaericPlacement = errorMsg(4) : exit Function
      case 5 : SphaericPlacement = errorMsg(5) : exit Function
      'case 6 : MsgBox errorMsg(6) : resume next
      'case 7 : MsgBox errorMsg(7) : resume next
      case else : SphaericPlacement = errorMsg(0) : exit Function
   end select

End Function
Back to top
View user's profile Send private message
Display posts from previous:   
Post new topic   Reply to topic    OOoForum.org Forum Index -> OpenOffice.org Code Snippets All times are GMT - 8 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group