void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
{
dAASSERT (n && p && q);
if (dFabs(n[2]) > M_SQRT1_2) {
// choose p in y-z plane
dReal a = n[1]*n[1] + n[2]*n[2];
dReal k = dRecipSqrt (a);
p[0] = 0;
p[1] = -n[2]*k;
p[2] = n[1]*k;
// set q = n x p
q[0] = a*k;
q[1] = -n[0]*p[2];
q[2] = n[0]*p[1];
}
else {
// choose p in x-y plane
dReal a = n[0]*n[0] + n[1]*n[1];
dReal k = dRecipSqrt (a);
p[0] = -n[1]*k;
p[1] = n[0]*k;
p[2] = 0;
// set q = n x p
q[0] = -n[2]*p[1];
q[1] = n[2]*p[0];
q[2] = a*k;
}
}
void dNormalize3 (dVector3 a)
{
dReal a0,a1,a2,aa0,aa1,aa2,l;
dAASSERT (a);
a0 = a[0];
a1 = a[1];
a2 = a[2];
aa0 = dFabs(a0);
aa1 = dFabs(a1);
aa2 = dFabs(a2);
if (aa1 > aa0) {
if (aa2 > aa1) {
goto aa2_largest;
}
else { // aa1 is largest
a0 /= aa1;
a2 /= aa1;
l = dRecipSqrt (a0*a0 + a2*a2 + 1);
a[0] = a0*l;
a[1] = dCopySign(l,a1);
a[2] = a2*l;
}
}
else {
if (aa2 > aa0) {
aa2_largest: // aa2 is largest
a0 /= aa2;
a1 /= aa2;
l = dRecipSqrt (a0*a0 + a1*a1 + 1);
a[0] = a0*l;
a[1] = a1*l;
a[2] = dCopySign(l,a2);
}
else { // aa0 is largest
if (aa0 <= 0) {
// dDEBUGMSG ("vector has zero size"); ... this messace is annoying
a[0] = 1; // if all a's are zero, this is where we'll end up.
a[1] = 0; // return a default unit length vector.
a[2] = 0;
return;
}
a1 /= aa0;
a2 /= aa0;
l = dRecipSqrt (a1*a1 + a2*a2 + 1);
a[0] = dCopySign(l,a0);
a[1] = a1*l;
a[2] = a2*l;
}
}
}
int _dSafeNormalize3 (dVector3 a)
{
dIASSERT(a);
dReal l = dCalcVectorLengthSquare3(a);
if (l > dEpsilon)
{
dScaleVector3r4(a, dRecipSqrt(l));
return 1;
}
a[0] = 1; // if all a's are zero, this is where we'll end up.
a[1] = 0; // return a default unit length vector.
a[2] = 0;
return 0;
}
int _dSafeNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dCalcVectorLengthSquare4(a);
if (l > 0) {
l = dRecipSqrt(l);
dScaleVector4(a, l);
return 1;
}
else {
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
return 0;
}
}
static void make_sure_plane_normal_has_unit_length (dxPlane *g)
{
dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2];
if (l > 0) {
l = dRecipSqrt(l);
g->p[0] *= l;
g->p[1] *= l;
g->p[2] *= l;
g->p[3] *= l;
}
else {
g->p[0] = 1;
g->p[1] = 0;
g->p[2] = 0;
g->p[3] = 0;
}
}
int _dSafeNormalize3 (dVector3 a)
{
dAASSERT (a);
int idx;
dReal aa[3], l;
aa[0] = dFabs(a[0]);
aa[1] = dFabs(a[1]);
aa[2] = dFabs(a[2]);
if (aa[1] > aa[0]) {
if (aa[2] > aa[1]) { // aa[2] is largest
idx = 2;
}
else { // aa[1] is largest
idx = 1;
}
}
else {
if (aa[2] > aa[0]) {// aa[2] is largest
idx = 2;
}
else { // aa[0] might be the largest
if (aa[0] <= 0) { // aa[0] might is largest
a[0] = 1; // if all a's are zero, this is where we'll end up.
a[1] = 0; // return a default unit length vector.
a[2] = 0;
return 0;
}
else {
idx = 0;
}
}
}
a[0] /= aa[idx];
a[1] /= aa[idx];
a[2] /= aa[idx];
l = dRecipSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
a[0] *= l;
a[1] *= l;
a[2] *= l;
return 1;
}
void dNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dDOT(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
}
else {
dDEBUGMSG ("vector has zero size");
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
}
}
EXPORT_C void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dReal l = dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az);
if (l > REAL(0.0)) {
angle = dMUL(angle,REAL(0.5));
q[0] = dCos (angle);
l = dMUL(dReal(dSin(angle)),dRecipSqrt(l));
q[1] = dMUL(ax,l);
q[2] = dMUL(ay,l);
q[3] = dMUL(az,l);
}
else {
q[0] = REAL(1.0);
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
int _dSafeNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dDOT(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
return 1;
}
else {
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
return 0;
}
}
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dAASSERT (q);
dReal l = ax*ax + ay*ay + az*az;
if (l > REAL(0.0)) {
angle *= REAL(0.5);
q[0] = dCos (angle);
l = dSin(angle) * dRecipSqrt(l);
q[1] = ax*l;
q[2] = ay*l;
q[3] = az*l;
}
else {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
int _dSafeNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dDOT(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
return 1;
}
else {
dIASSERT(false); // high_festiva was here!
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
return 0;
}
}
