63#define SLERP_EPSILON 0.001
65static int _nxt[3] = { 1 , 2 , 0 };
71static float project_to_sphere(
float,
float,
float);
92 float s = sinf(angle/2);
93 float c = cosf(angle/2);
163 float cos_t = qto.
X *
X + qto.
Y *
Y + qto.
Z *
Z + qto.
W *
W;
200 if ((x0 == x1) && (y0 == y1)) {
209 p1[2] = project_to_sphere(sphsize, x0, y0);
213 p2[2] = project_to_sphere(sphsize, x1, y1);
221 t = d.
Length() / (2.0f * sphsize);
224 if (t > 1.0f) t = 1.0f;
225 if (t < -1.0f) t = -1.0f;
226 phi = 2.0f * asin(t);
254 q.Scale(sinf(phi / 2.0f));
255 q[3] = cosf(phi / 2.0f);
284 cos_t = p.
X *
q.X + p.
Y *
q.Y + p.
Z *
q.Z + p.
W *
q.W;
305 oo_sin_t = 1.0 / sin_t;
306 beta = sinf(theta - alpha*theta) * oo_sin_t;
307 alpha = sinf(alpha*theta) * oo_sin_t;
315 res.
X = beta*p.
X + alpha*
q.X;
316 res.
Y = beta*p.
Y + alpha*
q.Y;
317 res.
Z = beta*p.
Z + alpha*
q.Z;
318 res.
W = beta*p.
W + alpha*
q.W;
340 assert(slerpinfo !=
NULL);
343 cos_t = p.
X *
q.X + p.
Y *
q.Y + p.
Z *
q.Z + p.
W *
q.W;
348 slerpinfo->
Flip =
true;
350 slerpinfo->
Flip =
false;
356 slerpinfo->
Theta = 0.0f;
357 slerpinfo->
SinT = 0.0f;
361 slerpinfo->
Linear =
false;
362 slerpinfo->
Theta = acos(cos_t);
363 slerpinfo->
SinT = sinf(slerpinfo->
Theta);
394 oo_sin_t = 1.0 / slerpinfo->
Theta;
395 beta = sin(slerpinfo->
Theta - alpha*slerpinfo->
Theta) * oo_sin_t;
396 alpha = sin(alpha*slerpinfo->
Theta) * oo_sin_t;
399 if (slerpinfo->
Flip) {
404 res.
X = beta*p.
X + alpha*
q.X;
405 res.
Y = beta*p.
Y + alpha*
q.Y;
406 res.
Z = beta*p.
Z + alpha*
q.Z;
407 res.
W = beta*p.
W + alpha*
q.W;
426 oo_sin_t = 1.0 / slerpinfo->
Theta;
427 beta = sin(slerpinfo->
Theta - alpha*slerpinfo->
Theta) * oo_sin_t;
428 alpha = sin(alpha*slerpinfo->
Theta) * oo_sin_t;
431 if (slerpinfo->
Flip) {
435 set_q->
X = beta*p.
X + alpha*
q.X;
436 set_q->
Y = beta*p.
Y + alpha*
q.Y;
437 set_q->
Z = beta*p.
Z + alpha*
q.Z;
438 set_q->
W = beta*p.
W + alpha*
q.W;
461 tr = mat[0][0] + mat[1][1] + mat[2][2];
469 q[0] = (mat[2][1] - mat[1][2]) * s;
470 q[1] = (mat[0][2] - mat[2][0]) * s;
471 q[2] = (mat[1][0] - mat[0][1]) * s;
476 if (mat[1][1] > mat[0][0]) i = 1;
477 if (mat[2][2] > mat[i][i]) i = 2;
481 s = sqrt((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
488 q[3] = ( mat[k][j] - mat[j][k] ) * s;
489 q[j] = ( mat[j][i] + mat[i][j] ) * s;
490 q[k] = ( mat[k][i] + mat[i][k] ) * s;
504 tr = mat[0][0] + mat[1][1] + mat[2][2];
512 q[0] = (mat[2][1] - mat[1][2]) * s;
513 q[1] = (mat[0][2] - mat[2][0]) * s;
514 q[2] = (mat[1][0] - mat[0][1]) * s;
519 if (mat[1][1] > mat[0][0]) i = 1;
520 if (mat[2][2] > mat[i][i]) i = 2;
525 s = sqrt( (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0);
533 q[3] = ( mat[k][j] - mat[j][k] ) * s;
534 q[j] = ( mat[j][i] + mat[i][j] ) * s;
535 q[k] = ( mat[k][i] + mat[i][k] ) * s;
548 tr = mat[0][0] + mat[1][1] + mat[2][2];
556 q[0] = (mat[2][1] - mat[1][2]) * s;
557 q[1] = (mat[0][2] - mat[2][0]) * s;
558 q[2] = (mat[1][0] - mat[0][1]) * s;
563 if (mat[1][1] > mat[0][0]) i = 1;
564 if (mat[2][2] > mat[i][i]) i = 2;
569 s = sqrt( (mat[i][i] - (mat[j][j]+mat[k][k])) + 1.0);
575 q[3] = ( mat[k][j] - mat[j][k] ) * s;
576 q[j] = ( mat[j][i] + mat[i][j] ) * s;
577 q[k] = ( mat[k][i] + mat[i][k] ) * s;
600 m[0][0] = (float)(1.0 - 2.0 * (
q[1] *
q[1] +
q[2] *
q[2]));
601 m[0][1] = (float)(2.0 * (
q[0] *
q[1] -
q[2] *
q[3]));
602 m[0][2] = (float)(2.0 * (
q[2] *
q[0] +
q[1] *
q[3]));
604 m[1][0] = (float)(2.0 * (
q[0] *
q[1] +
q[2] *
q[3]));
605 m[1][1] = (float)(1.0 - 2.0f * (
q[2] *
q[2] +
q[0] *
q[0]));
606 m[1][2] = (float)(2.0 * (
q[1] *
q[2] -
q[0] *
q[3]));
608 m[2][0] = (float)(2.0 * (
q[2] *
q[0] -
q[1] *
q[3]));
609 m[2][1] = (float)(2.0 * (
q[1] *
q[2] +
q[0] *
q[3]));
610 m[2][2] =(float)(1.0 - 2.0 * (
q[1] *
q[1] +
q[0] *
q[0]));
620 m[0][0] = (float)(1.0 - 2.0 * (
q[1] *
q[1] +
q[2] *
q[2]));
621 m[0][1] = (float)(2.0 * (
q[0] *
q[1] -
q[2] *
q[3]));
622 m[0][2] = (float)(2.0 * (
q[2] *
q[0] +
q[1] *
q[3]));
624 m[1][0] = (float)(2.0 * (
q[0] *
q[1] +
q[2] *
q[3]));
625 m[1][1] = (float)(1.0 - 2.0f * (
q[2] *
q[2] +
q[0] *
q[0]));
626 m[1][2] = (float)(2.0 * (
q[1] *
q[2] -
q[0] *
q[3]));
628 m[2][0] = (float)(2.0 * (
q[2] *
q[0] -
q[1] *
q[3]));
629 m[2][1] = (float)(2.0 * (
q[1] *
q[2] +
q[0] *
q[3]));
630 m[2][2] =(float)(1.0 - 2.0 * (
q[1] *
q[1] +
q[0] *
q[0]));
633 m[0][3] = m[1][3] = m[2][3] = 0.0f;
643 m[0][0] = (float)(1.0 - 2.0 * (
q[1] *
q[1] +
q[2] *
q[2]));
644 m[0][1] = (float)(2.0 * (
q[0] *
q[1] -
q[2] *
q[3]));
645 m[0][2] = (float)(2.0 * (
q[2] *
q[0] +
q[1] *
q[3]));
647 m[1][0] = (float)(2.0 * (
q[0] *
q[1] +
q[2] *
q[3]));
648 m[1][1] = (float)(1.0 - 2.0f * (
q[2] *
q[2] +
q[0] *
q[0]));
649 m[1][2] = (float)(2.0 * (
q[1] *
q[2] -
q[0] *
q[3]));
651 m[2][0] = (float)(2.0 * (
q[2] *
q[0] -
q[1] *
q[3]));
652 m[2][1] = (float)(2.0 * (
q[1] *
q[2] +
q[0] *
q[3]));
653 m[2][2] = (float)(1.0 - 2.0 * (
q[1] *
q[1] +
q[0] *
q[0]));
656 m[0][3] = m[1][3] = m[2][3] = 0.0f;
659 m[3][0] = m[3][1] = m[3][2] = 0.0f;
667 *
this = (*this) *
Quaternion(Vector3(1,0,0),theta);
673 *
this = (*this) *
Quaternion(Vector3(0,1,0),theta);
679 *
this = (*this) *
Quaternion(Vector3(0,0,1),theta);
682float project_to_sphere(
float r,
float x,
float y)
684 const float SQRT2 = 1.41421356f;
688 if (d < r * (SQRT2/(2.0f)))
701 X = ((float) (rand() & 0xFFFF)) / 65536.0f;
702 Y = ((float) (rand() & 0xFFFF)) / 65536.0f;
703 Z = ((float) (rand() & 0xFFFF)) / 65536.0f;
704 W = ((float) (rand() & 0xFFFF)) / 65536.0f;
WWINLINE Quaternion(void)
void Rotate_Y(float theta)
Quaternion & Make_Closest(const Quaternion &qto)
WWINLINE Quaternion & operator=(const Quaternion &source)
void Rotate_X(float theta)
void Rotate_Z(float theta)
WWINLINE float Length(void) const
static WWINLINE void Cross_Product(const Vector3 &a, const Vector3 &b, Vector3 *result)
static float Sqrt(float val)
Quaternion Cached_Slerp(const Quaternion &p, const Quaternion &q, float alpha, SlerpInfoStruct *slerpinfo)
void Slerp_Setup(const Quaternion &p, const Quaternion &q, SlerpInfoStruct *slerpinfo)
Matrix3 Build_Matrix3(const Quaternion &q)
Matrix3D Build_Matrix3D(const Quaternion &q)
Matrix4 Build_Matrix4(const Quaternion &q)
Quaternion Build_Quaternion(const Matrix3D &mat)
Quaternion Trackball(float x0, float y0, float x1, float y1, float sphsize)
Quaternion Slerp(const Quaternion &p, const Quaternion &q, float alpha)
Quaternion Axis_To_Quat(const Vector3 &a, float phi)