bertram
Power User
Joined: 13 Nov 2005
Posts: 52
|
 Posted: Sun Sep 03, 2006 2:44 pm Post subject: ooCalc as a tool for x3d and vrml rotations with quaternions |
 |
|
| 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 |
|
|
