Math-SO3
view release on metacpan or search on metacpan
void
inv_translate_vectors(...)
CODE:
if( items<1
|| SvTYPE(SvRV(ST(0)))!=SVt_PVMG)
{
croak(croak_inv_translate);
}
else
{
int i;
Math__SO3 so3;
double x,y,z, *vec;
so3=(Math__SO3)(SvPV(SvRV(ST(0)), PL_na));
for(i=1;i<items;i++)
{
if(!SvPOK(ST(i)))
{
croak(croak_inv_translate);
}
}
for(i=1;i<items;i++)
{
vec=(double *)SvPV(ST(i),PL_na);
x=vec[0];
y=vec[1];
z=vec[2];
/* for orthogonal matrices, X^T = X^-1 */
vec[0]=so3[0*3+0]*x+so3[1*3+0]*y+so3[2*3+0]*z;
vec[1]=so3[0*3+1]*x+so3[1*3+1]*y+so3[2*3+1]*z;
vec[2]=so3[0*3+2]*x+so3[1*3+2]*y+so3[2*3+2]*z;
}
}
# Return turning angle and direction vector corresponding to a given
# so3 matrix.
#
# We use two tricks:
#
# * the trace of a matrix is conjugation invariant, and (look at x rotation)
# here it's just 1+2*cos(alpha); this gives us alpha.
#
# * We use a (normalized) vector v1=(1,0,0)^T, turn it once to get v2,
# turn it once more to get v3;
# the normalized cross product (v3-v1)x(v2-v1) will be the rotation
# direction, unless it is too small; in this case, we redo the computation
# with v1=(0,1,0)^T.
# Note that this trick does not work with an angle of 180 degrees;
# here, we take an arbitrary vector and its rotated image and compute the
# normalized sum. Take a different vector if norm is zero. There are
# cases when we must do this three times.
#
# XXX have to put a bit more thought into this. Does it really work that way,
# or did I still miss a point?
void
turning_angle_and_dir(...)
PPCODE:
{
double angle, *dir, v2[3], v3[3], norm;
char *cdir;
if( (items!=1 && items != 2)
|| SvTYPE(SvRV(ST(0)))!=SVt_PVMG
|| 0==(cdir=alloca(1+3*sizeof(double))))
{
croak(croak_turning_angle_and_dir);
}
else
{
Math__SO3 so3;
so3=(Math__SO3)(SvPV(SvRV(ST(0)), PL_na));
cdir[3*sizeof(double)]=0;
dir=(double *)cdir;
angle=my_acos(0.5*(so3[0*3+0]+so3[1*3+1]+so3[2*3+2]-1));
if(angle==M_PI || angle==-M_PI)
{
/* first, try (1,0,0)^T */
v2[0]=so3[0*3+0];
v2[1]=so3[1*3+0];
v2[2]=so3[2*3+0];
dir[0]=v2[0]+1;
dir[1]=v2[1];
dir[2]=v2[2];
norm=sqrt(dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]);
if(norm<0.0001)
{
/* try (0,1,0)^T */
v2[0]=so3[0*3+1];
v2[1]=so3[1*3+1];
v2[2]=so3[2*3+1];
dir[0]=v2[0];
dir[1]=v2[1]+1;
dir[2]=v2[2];
norm=sqrt(dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]);
}
if(norm<0.0001)
{
/* lot of bad luck: (x,y,0)-plane "nearly" perpendicular to
turning direction. Now we simply could take z-direction,
but maybe this is a somewhat bad estimate; so,
we play the same game once again with the (1,0,1) vector.
*/
/* try (0,1,0)^T */
v2[0]=so3[0*3+0]+so3[0*3+2];
v2[1]=so3[1*3+0]+so3[1*3+2];
v2[2]=so3[2*3+0]+so3[2*3+2];
dir[0]=v2[0]+1;
if(norm<0.0001) /* can only happen for identity. */
{
dir[0]=0;
dir[0]=0;
dir[0]=1;
}
else
{
dir[0]/=norm;
dir[1]/=norm;
dir[2]/=norm;
}
}
EXTEND(sp,2);
if(items==2)
{
char *angle_spec;
angle_spec=(char *)SvPV(ST(1), PL_na);
if(angle_spec[0]=='d')
{
angle*=180/M_PI;
}
else if(angle_spec[0]!='r' && angle_spec[0]!=0)
{
croak(croak_turning_angle_and_dir);
}
}
PUSHs(sv_2mortal(newSVnv(angle)));
PUSHs(sv_2mortal(newSVpv(cdir,3*sizeof(double))));
}
}
# Standard euler angles
#
# alas, at first I tried to do all the unwinding in my head. Then, in
# the testing phase, I noticed that it worked "in most cases"; what I
# had gotten wrong were basically the constraints on the intermediate
# angle. False hubris. Now, let's do it the dumb way by formula:
# zxz Euler angles are (phi, theta, psi) = (p,q,r) {Euler's nomenclature}
# Therefore,
# ( cos r cos p - sin r cos q sin p | cos r sin p + sin r cos q cos p | sin r sin q )
# ( | | )
# Dz(r)Dx(q)Dz(p) = ( -sin r cos p - cos r cos q sin p | -sin r sin p + cos r cos q cos p | cos r sin q )
# ( | | )
# ( sin q sin p | -sin q cos p | cos q )
#
# What we do:
#
# (1) Determine q (theta) from [2][2] element.
# (2) Determine r (psi) from [0][2] and [1][2] elements. If theta=0, we can also set psi=0.
# (Since all that remains is a z-rotation or z-rotation with 180deg x-rotation,
# both characterized by a single angle)
# (3) Do some math to determine phi from [1][0] and [1][1] elements if theta=0,
# otherwise use [2][0] and [2][1] elements.
void
euler_angles_zxz(...)
PPCODE:
{
if( (items!=1 && items!=2)
|| SvTYPE(SvRV(ST(0)))!=SVt_PVMG)
{
croak(croak_euler_angles_zxz);
}
else
{
int i;
double phi, theta, psi, cos_theta, sin_theta, cos_psi, sin_psi, sin_phi, cos_phi;
Math__SO3 so3;
so3=(double *)(SvPV(SvRV(ST(0)),PL_na));
cos_theta=so3[2*3+2];
theta=my_acos(cos_theta);
sin_theta=sin(theta);
if(sin_theta==0)
{
psi=sin_psi=0; cos_psi=1;
/* note that cos_psi=1, cos_theta=+-1, sin_psi=0 here; looking close reveals that
the [0][0] element is just cos_phi, and [0][1] element is sin_phi */
sin_phi=so3[0*3+1];
cos_phi=so3[0*3+0];
phi=atan2(sin_phi, cos_phi);
}
else
{
sin_psi=so3[0*3+2]/sin_theta;
cos_psi=so3[1*3+2]/sin_theta;
psi=atan2(sin_psi, cos_psi);
sin_phi=so3[2*3+0]/sin_theta;
cos_phi=-so3[2*3+1]/sin_theta;
phi=atan2(sin_phi, cos_phi);
}
if(phi<0)phi+=2*M_PI;
if(psi<0)psi+=2*M_PI;
if(items==2)
{
char *angle_spec;
angle_spec=(char *)SvPV(ST(1), PL_na);
if(angle_spec[0]=='d')
{
phi*=180/M_PI;
theta*=180/M_PI;
psi*=180/M_PI;
}
else if(angle_spec[0]!='r' && angle_spec[0]!=0)
{
croak(croak_euler_angles_zxz);
}
}
EXTEND(sp,3);
PUSHs(sv_2mortal(newSVnv(phi)));
PUSHs(sv_2mortal(newSVnv(theta)));
PUSHs(sv_2mortal(newSVnv(psi)));
}
}
# this gives heading, pitch, roll. (h,p,r)
#
# Constraints: -90deg<pitch<=90deg
#
# ( cos r cos h - sin r sin p sin h | cos r sin h + sin r sin p cos h | - sin r cos p )
# ( )
# Dy(r)Dx(p)Dz(h) = ( -cos p sin h | cos p cos h | sin p )
# ( )
# ( sin r cos h + cos r sin p sin h | sin r sin h - cos r sin p cos h | cos r cos p )
#
# Strategy:
#
# (1) use [0][2] and [2][2] to determine abs cos p
# (2) use [1][2] to determine if p>=0 or p<0
# (3) if cos_p!=0, use [1][x] and [x][2] to determine pitch, roll
# (4) otherwise, things get bad.
# Case #1: sin_p>0 (approx. 1)
#
# [0][0]: cos r cos h - sin r sin h => cos (r+h)
# [0][2]: sin r cos h + cos r sin h => sin (r+h)
#
# Case #2: sin_p<0 (approx. 1)
#
# [0][0]: cos r cos -h - sin r sin -h => cos (r-h)
# [0][2]: sin r cos -h + cos r sin -h => sin (r-h)
#
# Heading does not make sense for an airplane going straight up or straight down.
# Therefore, we take this last angle to be all roll.
#
# => heading=0, roll=atan2(sin_roll, cos_roll)
void
euler_angles_yxz(...)
PPCODE:
{
int i;
double heading, pitch, roll, cos_heading, cos_pitch, cos_roll, sin_heading, sin_pitch, sin_roll;
Math__SO3 so3;
if( (items!=1 && items!=2)
|| SvTYPE(SvRV(ST(0)))!=SVt_PVMG)
{
croak(croak_euler_angles_zxz);
}
else
{
so3=(double *)(SvPV(SvRV(ST(0)),PL_na));
cos_pitch=sqrt(so3[0*3+2]*so3[0*3+2]+so3[2*3+2]*so3[2*3+2]); /* abs cos p */
pitch=my_acos(cos_pitch);
sin_pitch=so3[1*3+2];
if(sin_pitch<0)pitch=-pitch;
if(cos_pitch==0)
{
heading=0;
sin_roll=so3[0*3+2];
cos_roll=so3[0*3+0];
roll=atan2(sin_roll, cos_roll);
}
else
{
sin_heading=-so3[1*3+0]/cos_pitch;
cos_heading=so3[1*3+1]/cos_pitch;
sin_roll=-so3[0*3+2]/cos_pitch;
cos_roll=so3[2*3+2]/cos_pitch;
heading=atan2(sin_heading, cos_heading);
roll=atan2(sin_roll, cos_roll);
}
if(roll<0)roll+=2*M_PI;
if(heading<0)heading+=2*M_PI;
if(items==2)
{
char *angle_spec;
angle_spec=(char *)SvPV(ST(1), PL_na);
if(angle_spec[0]=='d')
{
heading*=180/M_PI;
pitch*=180/M_PI;
roll*=180/M_PI;
}
else if(angle_spec[0]!='r' && angle_spec[0]!=0)
{
croak(croak_euler_angles_zxz);
}
}
( run in 0.976 second using v1.01-cache-2.11-cpan-5511b514fd6 )