gim_geometry.h

ODE v0.8 很好用的多平台几何物理模拟库源代码,内含多個示例

   if ((det!=1.0f) && (det != 0.0f)) {				\
      det = 1.0f / det;						\
      p[0] *= det;						\
      p[1] *= det;						\
   }								\
}\


/** transform normal vector by inverse transpose of matrix
 * and then renormalize the vector
 *
 * This macro computes inverse transpose of matrix m,
 * and multiplies vector v into it, to yeild vector p
 * Vector p is then normalized.
 */
#define NORM_XFORM_2X2(p,m,v)					\
{								\
   double len;							\
								\
   /* do nothing if off-diagonals are zero and diagonals are 	\
    * equal */							\
   if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
      p[0] = m[1][1]*v[0] - m[1][0]*v[1];			\
      p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
								\
      len = p[0]*p[0] + p[1]*p[1];				\
      GIM_INV_SQRT(len,len);					\
      p[0] *= len;						\
      p[1] *= len;						\
   } else {							\
      VEC_COPY_2 (p, v);					\
   }								\
}\


/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define OUTER_PRODUCT_2X2(m,v,t)				\
{								\
   m[0][0] = v[0] * t[0];					\
   m[0][1] = v[0] * t[1];					\
								\
   m[1][0] = v[1] * t[0];					\
   m[1][1] = v[1] * t[1];					\
}\


/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define OUTER_PRODUCT_3X3(m,v,t)				\
{								\
   m[0][0] = v[0] * t[0];					\
   m[0][1] = v[0] * t[1];					\
   m[0][2] = v[0] * t[2];					\
								\
   m[1][0] = v[1] * t[0];					\
   m[1][1] = v[1] * t[1];					\
   m[1][2] = v[1] * t[2];					\
								\
   m[2][0] = v[2] * t[0];					\
   m[2][1] = v[2] * t[1];					\
   m[2][2] = v[2] * t[2];					\
}\


/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define OUTER_PRODUCT_4X4(m,v,t)				\
{								\
   m[0][0] = v[0] * t[0];					\
   m[0][1] = v[0] * t[1];					\
   m[0][2] = v[0] * t[2];					\
   m[0][3] = v[0] * t[3];					\
								\
   m[1][0] = v[1] * t[0];					\
   m[1][1] = v[1] * t[1];					\
   m[1][2] = v[1] * t[2];					\
   m[1][3] = v[1] * t[3];					\
								\
   m[2][0] = v[2] * t[0];					\
   m[2][1] = v[2] * t[1];					\
   m[2][2] = v[2] * t[2];					\
   m[2][3] = v[2] * t[3];					\
								\
   m[3][0] = v[3] * t[0];					\
   m[3][1] = v[3] * t[1];					\
   m[3][2] = v[3] * t[2];					\
   m[3][3] = v[3] * t[3];					\
}\


/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define ACCUM_OUTER_PRODUCT_2X2(m,v,t)				\
{								\
   m[0][0] += v[0] * t[0];					\
   m[0][1] += v[0] * t[1];					\
								\
   m[1][0] += v[1] * t[0];					\
   m[1][1] += v[1] * t[1];					\
}\


/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define ACCUM_OUTER_PRODUCT_3X3(m,v,t)				\
{								\
   m[0][0] += v[0] * t[0];					\
   m[0][1] += v[0] * t[1];					\
   m[0][2] += v[0] * t[2];					\
								\
   m[1][0] += v[1] * t[0];					\
   m[1][1] += v[1] * t[1];					\
   m[1][2] += v[1] * t[2];					\
								\
   m[2][0] += v[2] * t[0];					\
   m[2][1] += v[2] * t[1];					\
   m[2][2] += v[2] * t[2];					\
}\


/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define ACCUM_OUTER_PRODUCT_4X4(m,v,t)				\
{								\
   m[0][0] += v[0] * t[0];					\
   m[0][1] += v[0] * t[1];					\
   m[0][2] += v[0] * t[2];					\
   m[0][3] += v[0] * t[3];					\
								\
   m[1][0] += v[1] * t[0];					\
   m[1][1] += v[1] * t[1];					\
   m[1][2] += v[1] * t[2];					\
   m[1][3] += v[1] * t[3];					\
								\
   m[2][0] += v[2] * t[0];					\
   m[2][1] += v[2] * t[1];					\
   m[2][2] += v[2] * t[2];					\
   m[2][3] += v[2] * t[3];					\
								\
   m[3][0] += v[3] * t[0];					\
   m[3][1] += v[3] * t[1];					\
   m[3][2] += v[3] * t[2];					\
   m[3][3] += v[3] * t[3];					\
}\


/** determinant of matrix
 *
 * Computes determinant of matrix m, returning d
 */
#define DETERMINANT_2X2(d,m)					\
{								\
   d = m[0][0] * m[1][1] - m[0][1] * m[1][0];			\
}\


/** determinant of matrix
 *
 * Computes determinant of matrix m, returning d
 */
#define DETERMINANT_3X3(d,m)					\
{								\
   d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);		\
   d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);	\
   d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);	\
}\


/** i,j,th cofactor of a 4x4 matrix
 *
 */
#define COFACTOR_4X4_IJ(fac,m,i,j) 				\
{								\
   int __ii[4], __jj[4], __k;						\
								\
   for (__k=0; __k<i; __k++) __ii[__k] = __k;				\
   for (__k=i; __k<3; __k++) __ii[__k] = __k+1;				\
   for (__k=0; __k<j; __k++) __jj[__k] = __k;				\
   for (__k=j; __k<3; __k++) __jj[__k] = __k+1;				\
								\
   (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] 	\
                            - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
   (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]]	\
                             - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
   (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]]	\
                             - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
								\
   __k = i+j;							\
   if ( __k != (__k/2)*2) {						\
      (fac) = -(fac);						\
   }								\
}\


/** determinant of matrix
 *
 * Computes determinant of matrix m, returning d
 */
#define DETERMINANT_4X4(d,m)					\
{								\
   double cofac;						\
   COFACTOR_4X4_IJ (cofac, m, 0, 0);				\
   d = m[0][0] * cofac;						\
   COFACTOR_4X4_IJ (cofac, m, 0, 1);				\
   d += m[0][1] * cofac;					\
   COFACTOR_4X4_IJ (cofac, m, 0, 2);				\
   d += m[0][2] * cofac;					\
   COFACTOR_4X4_IJ (cofac, m, 0, 3);				\
   d += m[0][3] * cofac;					\
}\


/** cofactor of matrix
 *
 * Computes cofactor of matrix m, returning a
 */
#define COFACTOR_2X2(a,m)					\
{								\
   a[0][0] = (m)[1][1];						\
   a[0][1] = - (m)[1][0];						\
   a[1][0] = - (m)[0][1];						\
   a[1][1] = (m)[0][0];						\
}\


/** cofactor of matrix
 *
 * Computes cofactor of matrix m, returning a
 */
#define COFACTOR_3X3(a,m)					\
{								\
   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
   a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
   a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
   a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
   a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
   a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
   a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
}\


/** cofactor of matrix
 *
 * Computes cofactor of matrix m, returning a
 */
#define COFACTOR_4X4(a,m)					\
{								\
   int i,j;							\
								\
   for (i=0; i<4; i++) {					\
      for (j=0; j<4; j++) {					\
         COFACTOR_4X4_IJ (a[i][j], m, i, j);			\
      }								\
   }								\
}\


/** adjoint of matrix
 *
 * Computes adjoint of matrix m, returning a
 * (Note that adjoint is just the transpose of the cofactor matrix)
 */
#define ADJOINT_2X2(a,m)					\
{								\
   a[0][0] = (m)[1][1];						\
   a[1][0] = - (m)[1][0];						\
   a[0][1] = - (m)[0][1];						\
   a[1][1] = (m)[0][0];						\
}\


/** adjoint of matrix
 *
 * Computes adjoint of matrix m, returning a
 * (Note that adjoint is just the transpose of the cofactor matrix)
 */
#define ADJOINT_3X3(a,m)					\
{								\
   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
   a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
   a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
   a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
   a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
   a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
   a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
}\


/** adjoint of matrix
 *
 * Computes adjoint of matrix m, returning a
 * (Note that adjoint is just the transpose of the cofactor matrix)
 */
#define ADJOINT_4X4(a,m)					\
{								\
   char _i_,_j_;							\
								\
   for (_i_=0; _i_<4; _i_++) {					\
      for (_j_=0; _j_<4; _j_++) {					\
         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
      }								\
   }								\
}\


/** compute adjoint of matrix and scale
 *
 * Computes adjoint of matrix m, scales it by s, returning a
 */
#define SCALE_ADJOINT_2X2(a,s,m)				\
{								\
   a[0][0] = (s) * m[1][1];					\
   a[1][0] = - (s) * m[1][0];					\
   a[0][1] = - (s) * m[0][1];					\
   a[1][1] = (s) * m[0][0];					\
}\


/** compute adjoint of matrix and scale
 *
 * Computes adjoint of matrix m, scales it by s, returning a
 */
#define SCALE_ADJOINT_3X3(a,s,m)				\
{								\
   a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);	\
   a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);	\
   a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);	\
								\
   a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);	\
   a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);	\
   a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);	\
								\
   a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);	\
   a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);	\
   a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);	\
}\


/** compute adjoint of matrix and scale
 *
 * Computes adjoint of matrix m, scales it by s, returning a
 */
#define SCALE_ADJOINT_4X4(a,s,m)				\
{								\
   char _i_,_j_; \
   for (_i_=0; _i_<4; _i_++) {					\
      for (_j_=0; _j_<4; _j_++) {					\
         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
         a[_j_][_i_] *= s;						\
      }								\
   }								\
}\

/** inverse of matrix
 *
 * Compute inverse of matrix a, returning determinant m and
 * inverse b
 */
#define INVERT_2X2(b,det,a)			\
{						\
   GREAL _tmp_;					\
   DETERMINANT_2X2 (det, a);			\
   _tmp_ = 1.0 / (det);				\
   SCALE_ADJOINT_2X2 (b, _tmp_, a);		\
}\


/** inverse of matrix
 *
 * Compute inverse of matrix a, returning determinant m and
 * inverse b
 */
#define INVERT_3X3(b,det,a)			\
{						\
   GREAL _tmp_;					\
   DETERMINANT_3X3 (det, a);			\
   _tmp_ = 1.0 / (det);				\
   SCALE_ADJOINT_3X3 (b, _tmp_, a);		\
}\


/** inverse of matrix
 *
 * Compute inverse of matrix a, returning determinant m and
 * inverse b
 */
#define INVERT_4X4(b,det,a)			\
{						\
   GREAL _tmp_;					\
   DETERMINANT_4X4 (det, a);			\
   _tmp_ = 1.0 / (det);				\
   SCALE_ADJOINT_4X4 (b, _tmp_, a);		\
}\

//! @}

/*! \defgroup BOUND_AABB_OPERATIONS
*/
//! @{

//!Initializes an AABB
#define INVALIDATE_AABB(aabb) {\
    (aabb).minX = G_REAL_INFINITY;\
    (aabb).maxX = -G_REAL_INFINITY;\
    (aabb).minY = G_REAL_INFINITY;\
    (aabb).maxY = -G_REAL_INFINITY;\
    (aabb).minZ = G_REAL_INFINITY;\
    (aabb).maxZ = -G_REAL_INFINITY;\
}\

#define AABB_GET_MIN(aabb,vmin) {\
    vmin[0] = (aabb).minX;\
    vmin[1] = (aabb).minY;\
    vmin[2] = (aabb).minZ;\
}\

#define AABB_GET_MAX(aabb,vmax) {\
    vmax[0] = (aabb).maxX;\
    vmax[1] = (aabb).maxY;\
    vmax[2] = (aabb).maxZ;\
}\

//!Copy boxes
#define AABB_COPY(dest_aabb,src_aabb)\
{\
    (dest_aabb).minX = (src_aabb).minX;\
    (dest_aabb).maxX = (src_aabb).maxX;\
    (dest_aabb).minY = (src_aabb).minY;\
    (dest_aabb).maxY = (src_aabb).maxY;\
    (dest_aabb).minZ = (src_aabb).minZ;\
    (dest_aabb).maxZ = (src_aabb).maxZ;\
}\

//! Computes an Axis aligned box from  a triangle
#define COMPUTEAABB_FOR_TRIANGLE(aabb,V1,V2,V3) {\
    (aabb).minX = MIN3(V1[0],V2[0],V3[0]);\
    (aabb).maxX = MAX3(V1[0],V2[0],V3[0]);\
    (aabb).minY = MIN3(V1[1],V2[1],V3[1]);\
    (aabb).maxY = MAX3(V1[1],V2[1],V3[1]);\
    (aabb).minZ = MIN3(V1[2],V2[2],V3[2]);\
    (aabb).maxZ = MAX3(V1[2],V2[2],V3[2]);\
}\

//! Merge two boxes to destaabb
#define MERGEBOXES(destaabb,aabb) {\
    (destaabb).minX = MIN((aabb).minX,(destaabb).minX);\