clumaths.c

00001 #include <clu.h>
00002 
00003 CLvertex* cluVertexAdd(CLvertex* vf, const CLnormal* n)
00004 {
00005   vf->x += n->i;
00006   vf->y += n->j;
00007   vf->z += n->k;
00008 
00009   return vf;
00010 }
00011 
00012 CLvertex* cluVertexSubtract(CLvertex* vf, const CLnormal* n)
00013 {
00014   vf->x -= n->i;
00015   vf->y -= n->j;
00016   vf->z -= n->k;
00017 
00018   return vf;
00019 }
00020 
00021 CLvertex* cluVertexTransform(CLvertex* vf, const CLmatrix* m)
00022 {
00023   CLvertex temp;
00024   GLfloat w;
00025 
00026   clCopyVertex(&temp, vf); /* temp = *vf; also works :) */
00027 
00028   vf->x = m->m00 * temp.x;
00029   vf->y = m->m01 * temp.x;
00030   vf->z = m->m02 * temp.x;
00031   w = m->m03;
00032 
00033   vf->x += m->m10 * temp.y;
00034   vf->y += m->m11 * temp.y;
00035   vf->z += m->m12 * temp.y;  
00036   w += m->m13;
00037 
00038   vf->x += m->m20 * temp.z;
00039   vf->y += m->m21 * temp.z;
00040   vf->z += m->m22 * temp.z;
00041   w += m->m23;
00042 
00043   /* homogenous coordinate for a point is 1, so add final column */
00044   vf->x += m->m30;
00045   vf->y += m->m31;
00046   vf->z += m->m32;
00047   w += m->m33;
00048 
00049   if (w == 0)  /* if w == 0 here, the point is now a vector! */
00050     return 0; /* return NULL? */
00051   
00052   vf->x /= w;
00053   vf->y /= w;
00054   vf->z /= w;
00055   
00056   return vf;
00057 }
00058 
00059 GLfloat  cluVertexDifference(CLvertex* v0, CLvertex* v1)
00060 {
00061     CLnormal difference;
00062 
00063     cluNormalDifference(&difference, v0, v1);
00064 
00065     return cluNormalMagnitude(&difference);
00066 }
00067 
00068 CLnormal* cluNormalAdd(CLnormal* nf, const CLnormal* n)
00069 {
00070   nf->i += n->i;
00071   nf->j += n->j;
00072   nf->k += n->k;
00073   
00074   return nf;
00075 }
00076 
00077 CLnormal* cluNormalSubtract(CLnormal* nf, const CLnormal* n)
00078 {
00079   nf->i -= n->i;
00080   nf->j -= n->j;
00081   nf->k -= n->k;
00082   
00083   return nf;
00084 }
00085 
00086 CLnormal* cluNormalScale(CLnormal* nf, GLfloat s)
00087 {
00088   nf->i *= s;
00089   nf->j *= s;
00090   nf->k *= s;
00091   
00092   return nf;
00093 }
00094 
00095 CLnormal* cluNormalTransform(CLnormal* nf, const CLmatrix* m)
00096 {
00097   CLnormal temp;
00098   
00099   clCopyNormal(&temp, nf); /* temp = *nf; */
00100   
00101   nf->i = m->m00 * temp.i;
00102   nf->j = m->m01 * temp.i;
00103   nf->k = m->m02 * temp.i;
00104 
00105   nf->i += m->m10 * temp.j;
00106   nf->j += m->m11 * temp.j;
00107   nf->k += m->m12 * temp.j;  
00108   
00109   nf->i += m->m20 * temp.k;
00110   nf->j += m->m21 * temp.k;
00111   nf->k += m->m22 * temp.k;
00112   
00113   /* homogenous coordinate for a vector is 0, so ignore final column */
00114   
00115   return nf;
00116 }
00117 
00118 CLnormal* cluNormalNormalise(CLnormal* nf)
00119 {
00120   GLfloat m;
00121 
00122   m = cluNormalMagnitude(nf);
00123 
00124   if (m == 0.0f)
00125     return 0;
00126   
00127   nf->i /= m;
00128   nf->j /= m;
00129   nf->k /= m;
00130   
00131   return nf;
00132 }
00133 
00134 GLfloat cluNormalMagnitude(const CLnormal* n)
00135 {
00136     return (GLfloat)sqrt(n->i * n->i + n->j * n->j + n->k * n->k);
00137 }
00138 
00139 CLnormal* cluNormalDifference(CLnormal* nf,
00140                               const CLvertex* const v0,
00141                               const CLvertex* const v1)
00142 {
00143   nf->i = v0->x - v1->x;
00144   nf->j = v0->y - v1->y;
00145   nf->k = v0->z - v1->z;
00146     
00147   return nf;
00148 }
00149 
00150 GLfloat cluNormalDotProduct(const CLnormal* n0, const CLnormal* n1)
00151 {
00152   return n0->i * n1->i + n0->j * n1->j + n0->k * n1->k;
00153 }
00154 
00155 CLnormal* cluNormalCrossProduct(CLnormal* nf,
00156                                 const CLnormal* n0, const CLnormal* n1)
00157 {
00158   nf->i = n0->j * n1->k - n0->k * n1->j;
00159   nf->j = n0->k * n1->i - n0->i * n1->k;
00160   nf->k = n0->i * n1->j - n0->j * n1->i;
00161   
00162   return nf;
00163 }
00164 
00165 CLmatrix* cluMatrixTranslate(CLmatrix* mf, const CLnormal* n)
00166 {
00167   mf->m30 += n->i;
00168   mf->m31 += n->j;
00169   mf->m32 += n->k;
00170 
00171   return mf;
00172 }
00173 
00174 /* pre-multiply mf by m: 
00175 
00176      mf = m * mf
00177 
00178    nickl - changed to not assume homogeneous
00179 */
00180 CLmatrix* cluMatrixTransform(CLmatrix* mf, const CLmatrix* m)
00181 {
00182   CLmatrix temp;
00183 
00184   clCopyMatrix(&temp, mf);
00185 
00186   mf->m00 = m->m00 * temp.m00 + m->m10 * temp.m01 + m->m20 * temp.m02 + m->m30 * temp.m03;
00187   mf->m01 = m->m01 * temp.m00 + m->m11 * temp.m01 + m->m21 * temp.m02 + m->m31 * temp.m03;
00188   mf->m02 = m->m02 * temp.m00 + m->m12 * temp.m01 + m->m22 * temp.m02 + m->m32 * temp.m03;
00189   mf->m03 = m->m03 * temp.m00 + m->m13 * temp.m01 + m->m23 * temp.m02 + m->m33 * temp.m03;
00190 
00191   mf->m10 = m->m00 * temp.m10 + m->m10 * temp.m11 + m->m20 * temp.m12 + m->m30 * temp.m13;
00192   mf->m11 = m->m01 * temp.m10 + m->m11 * temp.m11 + m->m21 * temp.m12 + m->m31 * temp.m13;
00193   mf->m12 = m->m02 * temp.m10 + m->m12 * temp.m11 + m->m22 * temp.m12 + m->m32 * temp.m13;
00194   mf->m13 = m->m03 * temp.m10 + m->m13 * temp.m11 + m->m23 * temp.m12 + m->m33 * temp.m13;
00195 
00196   mf->m20 = m->m00 * temp.m20 + m->m10 * temp.m21 + m->m20 * temp.m22 + m->m30 * temp.m23;
00197   mf->m21 = m->m01 * temp.m20 + m->m11 * temp.m21 + m->m21 * temp.m22 + m->m31 * temp.m23;
00198   mf->m22 = m->m02 * temp.m20 + m->m12 * temp.m21 + m->m22 * temp.m22 + m->m32 * temp.m23;
00199   mf->m23 = m->m03 * temp.m20 + m->m13 * temp.m21 + m->m23 * temp.m22 + m->m33 * temp.m23;
00200 
00201   mf->m30 = m->m00 * temp.m30 + m->m10 * temp.m31 + m->m20 * temp.m32 + m->m30 * temp.m33;
00202   mf->m31 = m->m01 * temp.m30 + m->m11 * temp.m31 + m->m21 * temp.m32 + m->m31 * temp.m33;
00203   mf->m32 = m->m02 * temp.m30 + m->m12 * temp.m31 + m->m22 * temp.m32 + m->m32 * temp.m33;
00204   mf->m33 = m->m03 * temp.m30 + m->m13 * temp.m31 + m->m23 * temp.m32 + m->m33 * temp.m33;
00205 
00206   return mf;
00207 }
00208 
00209 /* input must be orthonormal and homogeneous 
00210 
00211    CHANGE THIS TO THE GENERAL CASE!!!
00212 */
00213 
00214 CLmatrix* cluMatrixInvert(CLmatrix* mf)
00215 {
00216   CLmatrix temp;
00217 
00218   clCopyMatrix(&temp, mf); /* m = *mf */
00219 
00220   /* transpose rotation matrix */
00221   mf->m10 = temp.m01;  mf->m20 = temp.m02; 
00222   mf->m01 = temp.m10;  mf->m21 = temp.m12; 
00223   mf->m02 = temp.m20;  mf->m12 = temp.m21;
00224   
00225   /* calculate relative translation */
00226   mf->m30 = temp.m30 * -mf->m00 + temp.m31 * -mf->m10 + temp.m32 * -mf->m20;
00227   mf->m31 = temp.m30 * -mf->m01 + temp.m31 * -mf->m11 + temp.m32 * -mf->m21;
00228   mf->m32 = temp.m30 * -mf->m02 + temp.m31 * -mf->m12 + temp.m32 * -mf->m22;
00229   
00230   /* no need to set homogeneous coordinates, assume orthonormal input */
00231   
00232   return mf;
00233 }
00234 
00235 CLUquaternion* cluQuaternionAdd(CLUquaternion* qf, const CLUquaternion* q)
00236 {
00237   qf->x += q->x;
00238   qf->y += q->y;
00239   qf->z += q->z;
00240   qf->w += q->w;
00241 
00242   return qf;
00243 }
00244 
00245 CLUquaternion* cluQuaternionSubtract(CLUquaternion* qf,
00246                                      const CLUquaternion* q)
00247 {
00248   qf->x -= q->x;
00249   qf->y -= q->y;
00250   qf->z -= q->z;
00251   qf->w -= q->w;
00252 
00253   return qf;
00254 }
00255 
00256 CLUquaternion* cluQuaternionScale(CLUquaternion* qf, GLfloat s)
00257 {
00258   qf->x *= s;
00259   qf->y *= s;
00260   qf->z *= s;
00261   qf->w *= s;
00262   
00263   return qf;
00264 }
00265 
00266 CLUquaternion* cluQuaternionPower(CLUquaternion* qf, GLfloat n)
00267 {
00268   GLfloat angle;
00269   CLnormal axis;
00270 
00271   angle = cluQuaternionAngle(qf);
00272   cluSetNormalQuaternionAxis(&axis, qf);
00273 
00274   return cluSetQuaternionAxisAngle(qf, &axis, n * angle);
00275 }
00276 
00277 GLfloat cluQuaternionDotProduct(const CLUquaternion* q0,
00278                                 const CLUquaternion* q1)
00279 {
00280   return q0->x * q1->x + q0->y * q1->y + q0->z * q1->z + q0->w * q1->w;
00281 }
00282 
00283 CLUquaternion* cluQuaternionMultiply(CLUquaternion* qf, const CLUquaternion* q)
00284 {
00285   CLUquaternion temp;
00286 
00287   cluCopyQuaternion(&temp, qf);
00288 
00289   qf->x = temp.w * q->x + temp.x * q->w + temp.y * q->z - temp.z * q->y;
00290   qf->y = temp.w * q->y + temp.y * q->w + temp.z * q->x - temp.x * q->z;
00291   qf->z = temp.w * q->z + temp.z * q->w + temp.x * q->y - temp.y * q->x;
00292   qf->w = temp.w * q->w - temp.x * q->x - temp.y * q->y - temp.z * q->z;
00293 
00294   return qf;
00295 }
00296 
00297 CLUquaternion* cluQuaternionNormalise(CLUquaternion* qf)
00298 {
00299   GLfloat m;
00300 
00301   m = cluQuaternionMagnitude(qf);
00302 
00303   if (m == 0)
00304     return 0;
00305   
00306   qf->x /= m;
00307   qf->y /= m;
00308   qf->z /= m;
00309   qf->w /= m;
00310   
00311   return qf;
00312 }
00313 
00314 GLfloat cluQuaternionMagnitude(const CLUquaternion* q)
00315 {
00316   return sqrt(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
00317 }
00318 
00319 GLfloat cluQuaternionAngle(const CLUquaternion* q)
00320 {
00321   return CL_RAD2DEG(2 * acos(q->w));
00322 }
00323 
00324 CLUquaternion* cluQuaternionConjugate(CLUquaternion* qf)
00325 {
00326   qf->x = -qf->x;
00327   qf->y = -qf->y;
00328   qf->z = -qf->z;
00329   
00330   return qf;
00331 }
00332 
00333 CLUquaternion* cluQuaternionInvert(CLUquaternion* qf)
00334 {
00335   return cluQuaternionConjugate(cluQuaternionNormalise(qf));
00336 }
00337 
00338 GLboolean cluQuaternionEquals(const CLUquaternion* q0,
00339                               const CLUquaternion* q1,
00340                               GLfloat threshold)
00341 {
00342     GLfloat xDiff, yDiff, zDiff, wDiff, totalDiff;
00343     xDiff = q0->x - q1->x;
00344     yDiff = q0->y - q1->y;
00345     zDiff = q0->z - q1->z;
00346     wDiff = q0->w - q1->w;
00347 
00348     totalDiff = fabs(xDiff) +   fabs(yDiff) +   fabs(zDiff) +   fabs(wDiff);
00349 
00350     if(totalDiff < threshold)
00351         return GL_TRUE;
00352 
00353     return GL_FALSE;
00354 
00355 }
00356 
00357 CLUplane* cluPlaneNormalise(CLUplane* plane)
00358 {
00359     double l;
00360 
00361     l = sqrt(plane->a * plane->a + plane->b * plane->b + plane->c * plane->c);
00362     
00363     plane->a /= l;
00364     plane->b /= l;
00365     plane->c /= l;
00366     plane->d /= l;
00367 
00368     return plane;
00369 }
00370 
00371 CLUplane* cluPlaneTransform(CLUplane* pf, const CLmatrix* m)
00372 {
00373   CLUplane temp;
00374   
00375   cluCopyPlane(&temp, pf);
00376   
00377   pf->a = m->m00 * temp.a;
00378   pf->b = m->m01 * temp.a;
00379   pf->c = m->m02 * temp.a;
00380 
00381   pf->a += m->m10 * temp.b;
00382   pf->b += m->m11 * temp.b;
00383   pf->c += m->m12 * temp.b;  
00384   
00385   pf->a += m->m20 * temp.c;
00386   pf->b += m->m21 * temp.c;
00387   pf->c += m->m22 * temp.c;
00388 
00389   pf->d -= pf->a * m->m30;
00390   pf->d -= pf->b * m->m31;
00391   pf->d -= pf->c * m->m32;
00392 
00393   return pf;
00394 }
00395 
00396 CLUray* cluRayTransform(CLUray* ray, const CLmatrix* matrix)
00397 {
00398     cluVertexTransform(&ray->origin, matrix);
00399     cluNormalTransform(&ray->direction, matrix);
00400 
00401     return ray;
00402 }
00403 
00404 GLfloat cluAlignedBoxWidth(const CLUalignedbox* box)
00405 {
00406     return box->max.x - box->min.x;
00407 }
00408 
00409 GLfloat cluAlignedBoxHeight(const CLUalignedbox* box)
00410 {
00411     return box->max.y - box->min.y;
00412 }
00413 
00414 GLfloat cluAlignedBoxDepth(const CLUalignedbox* box)
00415 {
00416     return box->max.z - box->min.z;
00417 }
00418 
00419 CLvertex* cluAlignedBoxOrigin(CLvertex* v, const CLUalignedbox* box)
00420 {
00421     return cluSetVertex(v,
00422                         (box->min.x + box->max.x) / 2.0f,
00423                         (box->min.y + box->max.y) / 2.0f,
00424                         (box->min.z + box->max.z) / 2.0f);
00425 }
00426 
00427 
00428 /*
00429   http://mathworld.wolfram.com/Point-PlaneDistance.html
00430 */
00435 GLfloat cluVertexDistance(const CLvertex* v0, const CLvertex* v1)
00436 {
00437   CLnormal normal;
00438   
00439   cluNormalDifference(&normal, v0, v1);
00440 
00441   return cluNormalMagnitude(&normal);
00442 }
00443 
00444 /*
00445   plane must be normalised
00446 */
00447 GLfloat cluPlaneDistance(const CLUplane* plane, const CLvertex* v)
00448 {
00449     return plane->a * v->x + plane->b * v->y + plane->c * v->z + plane->d;
00450 }
00451 
00452 /* true is *below* plane! */
00453 GLboolean cluPlaneIntersect(const CLUplane* plane, const CLvertex* vertex)
00454 { 
00455     return (cluPlaneDistance(plane, vertex) <= 0);
00456 }
00457 
00458 GLboolean cluPlaneIntersectSphere(const CLUplane* plane,
00459                                   const CLUsphere* sphere)
00460 {
00461     return cluPlaneDistance(plane, &sphere->origin) <= -sphere->radius;
00462 }
00463 
00464 /* true if *above* all planes (normals point into frustum) */
00465 GLboolean cluFrustumIntercept(const CLUfrustum* frustum,
00466                               const CLvertex* vertex)
00467 {
00468     if (cluPlaneIntersect(&frustum->left, vertex))
00469         return GL_FALSE;
00470     
00471     if (cluPlaneIntersect(&frustum->right, vertex))
00472         return GL_FALSE;
00473 
00474     if (cluPlaneIntersect(&frustum->bottom, vertex))
00475         return GL_FALSE;
00476 
00477     if (cluPlaneIntersect(&frustum->top, vertex))
00478         return GL_FALSE;
00479 
00480     if (cluPlaneIntersect(&frustum->near, vertex))
00481         return GL_FALSE;
00482 
00483     if (cluPlaneIntersect(&frustum->far, vertex))
00484         return GL_FALSE;
00485 
00486     return GL_TRUE;
00487 }
00488 
00489 /* true if *above* all planes (normals point into frustum) */
00490 GLboolean cluFrustumInterceptSphere(const CLUfrustum* frustum,
00491                                     const CLUsphere* sphere)
00492 {
00493     if (cluPlaneIntersectSphere(&frustum->left, sphere))
00494         return GL_FALSE;
00495 
00496     if (cluPlaneIntersectSphere(&frustum->right, sphere))
00497         return GL_FALSE;
00498 
00499     if (cluPlaneIntersectSphere(&frustum->bottom, sphere))
00500         return GL_FALSE;
00501 
00502     if (cluPlaneIntersectSphere(&frustum->top, sphere))
00503         return GL_FALSE;
00504 
00505     if (cluPlaneIntersectSphere(&frustum->near, sphere))
00506         return GL_FALSE;
00507 
00508     if (cluPlaneIntersectSphere(&frustum->far, sphere))
00509         return GL_FALSE;
00510 
00511     return GL_TRUE;
00512 }
00513 
00514 GLfloat   cluSphereDistance(const CLUsphere* sphere, const CLvertex* v)
00515 {
00516   return cluVertexDistance(&sphere->origin, v) - sphere->radius;
00517 }
00518 
00519 GLfloat   cluSphereDistanceSphere(const CLUsphere* sphere0,
00520                                   const CLUsphere* sphere1)
00521 {
00522   return (cluSphereDistance(sphere0, &sphere1->origin) - sphere1->radius);
00523 }
00524 
00525 /*
00526   \todo ur... this doesn't seem to make any sense whatsoever...
00527 
00528   3D Game Engine Design: volume 1, pg. 172
00529 */
00530 GLfloat cluRayDistanceSphere(const CLUray* ray, const CLUsphere* sphere)
00531 {
00532     CLnormal q; /* quadratic is: t^2 + 2 * a1 * t + a0 = 0 */
00533     GLfloat a0;
00534     GLfloat a1;
00535 
00536     cluNormalDifference(&q, &ray->origin, &sphere->origin);
00537     a0 = cluNormalDotProduct(&q, &q) - sphere->radius * sphere->radius;
00538     
00539     /* if inside the sphere */
00540     if (a0 <= 0)
00541         return GL_TRUE;
00542     
00543     /* angle from ray to sphere origin */
00544     a1 = cluNormalDotProduct(&ray->direction, &q);
00545 
00546     if (a1 >= 0)
00547         return GL_FALSE;
00548 
00549     /* if determinant is real */
00550     return ((a1 * a1) >= a0);
00551 }
00552 
00553 GLboolean cluSphereIntersect(const CLUsphere* sphere, const CLvertex* vertex)
00554 {
00555   return (cluSphereDistance(sphere, vertex) <= 0.0f);
00556 }
00557 
00561 GLboolean cluSphereIntersectSphere(const CLUsphere* sphere0,
00562                                    const CLUsphere* sphere1)
00563 {
00564   return (cluSphereDistanceSphere(sphere0, sphere1) <= 0.0f);
00565 }
00566 
00571 CLUsphere* cluSphereAddSphere(CLUsphere* sf, const CLUsphere* s0, const CLUsphere* s1)
00572 {
00573     CLnormal         difference_normal;           /* normal from the center of the 1st sphere to the center of the 2nd sphere, parent sphere will lie somewhere on this line */
00574     GLfloat          difference_normal_magnitude = 0.0f;
00575 
00576     GLfloat          s0_additions = 0.0f;
00577     GLfloat          s1_additions = 0.0f;
00578 
00579     GLfloat          total = 0.0f;
00580 
00581     /* get the vector from s0.origin to s1.origin */
00582     cluNormalDifference(&difference_normal, &s1->origin, &s0->origin);
00583     difference_normal_magnitude = cluNormalMagnitude(&difference_normal);
00584     
00585     /* consider additions for s0 */
00586     if (s1->radius > (difference_normal_magnitude + s0->radius))
00587     {
00588         s0_additions = s1->radius - difference_normal_magnitude;
00589     }
00590     else
00591     {
00592         s0_additions = s0->radius;
00593     }
00594 
00595     /* consider additions for s1 */
00596     if (s0->radius > (difference_normal_magnitude + s1->radius))
00597     {
00598         s1_additions = s0->radius - difference_normal_magnitude;
00599     }
00600     else
00601     {
00602         s1_additions = s1->radius;
00603     }
00604     
00605     /* now find center point */
00606     total = difference_normal_magnitude + s0_additions + s1_additions;
00607 
00608     cluNormalNormalise(&difference_normal);
00609 
00610     cluNormalScale(&difference_normal, total/2.0f - s0_additions);
00611     
00612     /* new center = s0 origin + difference_normal */
00613     clCopyVertex(&sf->origin, &s0->origin);
00614     cluVertexAdd(&sf->origin, &difference_normal);
00615 
00616     /* new radius = total/2.0f */
00617     sf->radius = total/2.0f;
00618 
00619     return sf;
00620 }
00621 
00626 GLboolean cluConeIntersect(const CLUcone* cone, const CLvertex* vertex)
00627 {
00628   CLnormal normal;
00629   
00630   cluNormalNormalise(cluNormalDifference(&normal, vertex, &cone->origin));
00631   
00632   return (cluNormalDotProduct(&cone->direction, &normal) <= cos(CL_DEG2RAD(cone->half_angle)));
00633 }
00634 
00635 GLboolean cluConeIntersectSphere(const CLUcone* cone, const CLUsphere* sphere)
00636 {
00637   CLUcone temp_cone;
00638   
00639   cluSetConeSphereVertex(&temp_cone, sphere, &cone->origin);
00640   
00641   return (acos((cluNormalDotProduct(&cone->direction, &temp_cone.direction)))
00642           <= CL_DEG2RAD(cone->half_angle + temp_cone.half_angle));
00643 }
00644 
00649 CLUcone* cluConeAddCone(CLUcone* cf, const CLUcone* c0, const CLUcone* c1)
00650 {
00651     GLfloat        difference_angle = 0.0f;     /* the angle between the directions of the cones (deg) */
00652     CLnormal       cross_product;               /* the cross product of the cones directions (axis)    */
00653 
00654     GLfloat        c0_additions = 0.0f;
00655     GLfloat        c1_additions = 0.0f;
00656 
00657     CLUquaternion  rotation_quaternion;         /* use axis and angle to create an orientation                      */
00658     CLmatrix       rotation_matrix;             /* translate the quaternion to a matrix so we can transform normals */
00659 
00660     GLfloat        total = 0.0f;
00661 
00662         /* the dot product is explicitly cropped to the range (-1.0, 1.0) to resolve any floating point         procision errors when passed to acos (which result in a NAN error) */
00663     difference_angle = CL_RAD2DEG(acos(CL_CROP(cluNormalDotProduct(&c0->direction, &c1->direction), -1.0f, 1.0f)));
00664 
00665     /* consider additions for c0 */
00666     if (c1->half_angle > (difference_angle + c0->half_angle))
00667     {
00668         c0_additions = c1->half_angle - difference_angle;
00669     }
00670     else
00671     {
00672         c0_additions = c0->half_angle;
00673     }
00674 
00675     /* consider additions for c1 */
00676     if (c0->half_angle > (difference_angle + c1->half_angle))
00677     {
00678         c1_additions = c0->half_angle - difference_angle;
00679     }
00680     else
00681     {
00682         c1_additions = c1->half_angle;
00683     }
00684     
00685     /* now find center point */
00686     total = difference_angle + c0_additions + c1_additions;
00687     
00688     /* init the structures */
00689     clDefaultNormal(&cross_product);
00690     cluDefaultQuaternion(&rotation_quaternion);
00691     clDefaultMatrix(&rotation_matrix);
00692     
00693     /* setup the rotation_matrix */
00694     cluNormalCrossProduct(&cross_product, &c0->direction, &c1->direction);
00695     cluSetQuaternionAxisAngle(&rotation_quaternion, &cross_product, total/2.0f - c0_additions);
00696     cluSetMatrixOrientation(&rotation_matrix, &rotation_quaternion);
00697     
00698     /* calc the parent cone by rotating one of the child cones */
00699     clCopyVertex(&cf->origin, &c0->origin);
00700     clCopyNormal(&cf->direction, &c1->direction);
00701     
00702     cluNormalTransform(&cf->direction, &rotation_matrix);
00703 
00704     cluNormalNormalise(&cf->direction);
00705 
00706     /* new angle = total/2.0f */
00707     cf->half_angle = total/2.0f;
00708 
00709     return cf;
00710 }
00711 
00712 #if 0
00713 /*
00714   3D Game Engine Design: volume 1, pg. 172
00715 */
00716 GLboolean cluRayInterceptSphere(const CLUray* ray, const CLUsphere* sphere)
00717 {
00718     CLnormal q; /* quadratic is: t^2 + 2 * a1 * t + a0 = 0 */
00719     GLfloat a0;
00720     GLfloat a1;
00721 
00722     cluNormalDifference(&q, &ray->origin, &sphere->origin);
00723     a0 = cluNormalDotProduct(&q, &q) - sphere->radius * sphere->radius;
00724     
00725     /* if inside the sphere */
00726     if (a0 <= 0)
00727         return GL_TRUE;
00728     
00729     /* angle from ray to sphere origin */
00730     a1 = cluNormalDotProduct(&ray->direction, &q);
00731 
00732     if (a1 >= 0)
00733         return GL_FALSE;
00734 
00735     /* if determinant is real */
00736     return ((a1 * a1) >= a0);
00737 }
00738 #endif
00739 
00755 bool cluRayIntersectSphere(float* t, const CLUray* ray, const CLUsphere* sphere)
00756 {
00757     CLnormal pminusc;
00758     float a, b, c;
00759     float discrm;
00760 
00761     cluNormalDifference(&pminusc, &ray->origin, &sphere->origin);
00762 
00763     a = cluNormalDotProduct(&ray->direction, &ray->direction);
00764     b = 2 * cluNormalDotProduct(&ray->direction, &pminusc);
00765     c = cluNormalDotProduct(&pminusc, &pminusc) - sphere->radius * sphere->radius;
00766     
00767     discrm = b * b - 4 * a * c;
00768 
00769     if (discrm > 0) /* should we use CL_EPSILON? */
00770     {
00771         *t = (-b - sqrt(discrm)) / (2 * a);
00772 
00773         return true;
00774     }
00775     
00776     if ((discrm < CL_EPSILON) && (discrm > CL_EPSILON))
00777     {
00778         *t = -b / (2 * a);
00779 
00780         return true;
00781     }
00782     
00783     return false;
00784 }
00785 
00804 bool cluRayIntersectTriangle(CLvertex* result,
00805                              const CLUray* ray,
00806                              const CLvertex* v0,
00807                              const CLvertex* v1,
00808                              const CLvertex* v2)
00809 {
00810     CLnormal edge1, edge2;
00811     CLnormal pvec, tvec, qvec;
00812     float det, inv_det;
00813     float t, u, v;
00814     
00815     /* find vectors for two edges sharing vert0 */
00816     cluNormalDifference(&edge1, v1, v0);
00817     cluNormalDifference(&edge2, v2, v0);
00818     
00819     /* begin calculating determinant - also used to calculate U parameter */
00820     cluNormalCrossProduct(&pvec, &ray->direction, &edge2);
00821     
00822     /* if determinant is near zero, ray lies in plane of triangle */
00823     det = cluNormalDotProduct(&edge1, &pvec);
00824     
00825 #if 1 /* define TEST_CULL if culling is desired */
00826     
00827     if (det < CL_EPSILON)
00828     {
00829         return false;
00830     }
00831     
00832     /* calculate distance from vert0 to ray origin */
00833     cluNormalDifference(&tvec, &ray->origin, v0);
00834     
00835     /* calculate U parameter and test bounds */
00836     u = cluNormalDotProduct(&tvec, &pvec);
00837     
00838     if (u < 0.0 || u > det)
00839     {
00840         return false;
00841     }
00842     
00843     /* prepare to test V parameter */
00844     cluNormalCrossProduct(&qvec, &tvec, &edge1);
00845     
00846     /* calculate V parameter and test bounds */
00847     v = cluNormalDotProduct(&ray->direction, &qvec);
00848    
00849     if (v < 0.0 || u + v > det)
00850     {
00851         return false;
00852     }
00853     
00854     /* calculate t, scale parameters, ray intersects triangle */
00855     t = cluNormalDotProduct(&edge2, &qvec);
00856     
00857     inv_det = 1.0 / det;
00858     
00859     t *= inv_det;
00860     u *= inv_det;
00861     v *= inv_det;
00862 
00863 #else /* the non-culling branch */
00864     
00865     if (det > -CL_EPSILON && det < CL_EPSILON)
00866     {
00867         return false;
00868     }
00869     
00870     inv_det = 1.0 / det;
00871 
00872     /* calculate distance from vert0 to ray origin */
00873     cluNormalDifference(&tvec, &ray->origin, v0);
00874 
00875     /* calculate U parameter and test bounds */
00876     u = cluNormalDotProduct(&tvec, &pvec) * inv_det;
00877 
00878     if (u < 0.0 || u > 1.0)
00879     {
00880         return false;
00881     }
00882 
00883     /* prepare to test V parameter */
00884     cluNormalCrossProduct(&qvec, &tvec, &edge1);
00885     
00886     /* calculate V parameter and test bounds */
00887     v = cluNormalDotProduct(&ray->direction, &qvec) * inv_det;
00888 
00889     if (v < 0.0 || u + v > 1.0)
00890     {
00891         return false;
00892     }
00893     
00894     /* calculate t, ray intersects triangle */
00895     t = cluNormalDotProduct(&edge2, &qvec) * inv_det;
00896     
00897 #endif
00898     
00899     result->x = t;
00900     result->y = u;
00901     result->z = v;
00902     
00903     return true;
00904 }
00905 
00920 CLvertex* cluSetVertexRayT(CLvertex* vf, const CLUray* ray, float t)
00921 {
00922     CLnormal offset;
00923     
00924     /* make a copy of the input ray */
00925     clCopyNormal(&offset, &ray->direction);
00926 
00927     /* scale the copy by t */
00928     cluNormalScale(&offset, t);
00929 
00930     /* make a copy of the input vertex */
00931     clCopyVertex(vf, &ray->origin);
00932 
00933     /* add the ray copy to the vertex copy */
00934     cluVertexAdd(vf, &offset);
00935     
00936     return vf;
00937 }
00938 
00952 CLUray* cluSetRayOriginDestination(CLUray* ray,
00953                                    CLvertex* origin,
00954                                    CLvertex* destination)
00955 {
00956     /* set ray origin from v0 */
00957     cluSetVertex(&ray->origin, origin->x, origin->y, origin->z);
00958     
00959     /* calc vector from v0->v1 */
00960 #if 0
00961     cluNormalDifference(&ray->direction, origin, destination);
00962 #else
00963     cluNormalDifference(&ray->direction, destination, origin);
00964 #endif
00965 
00966     return ray;
00967 }
00968 
00969 float cluSphereVolume(const CLUsphere* sphere)
00970 {
00971     return 4.0f / 3.0f * CL_PI * sphere->radius * sphere->radius * sphere->radius;
00972 }
00973 
00981 float cluConeVolume(const CLUcone* cone)
00982 {
00983     if (cone->half_angle >= 90.0f)
00984     {
00985         return INFINITY;
00986     }
00987     else
00988     {
00989         GLfloat height = 0.0f;
00990         GLfloat radius_base = 0.0f;
00991         
00992         height = cluNormalMagnitude(&cone->direction);
00993         radius_base = height * tan(CL_DEG2RAD(cone->half_angle));
00994         
00995         return 1.0f / 3.0f * CL_PI * radius_base * radius_base * height;
00996     }
00997 }

Generated on Thu Dec 27 13:53:42 2007 for CLU by  doxygen 1.4.6