gim_geometry.h

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

#ifndef GIM_VECTOR_H_INCLUDED
#define GIM_VECTOR_H_INCLUDED

/*! \file gim_geometry.h
\author Francisco Le髇
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon. C.C. 80087371.
email: [e-mail]

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/


#include "GIMPACT/gim_math.h"

/*! \defgroup GEOMETRIC_TYPES
\brief
Basic types and constants for geometry
*/
//! @{

//! Integer vector 2D
typedef GINT vec2i[2];
//! Integer vector 3D
typedef GINT vec3i[3];
//! Integer vector 4D
typedef GINT vec4i[4];

//! Float vector 2D
typedef GREAL vec2f[2];
//! Float vector 3D
typedef GREAL vec3f[3];
//! Float vector 4D
typedef GREAL vec4f[4];

//! Matrix 2D, row ordered
typedef GREAL mat2f[2][2];
//! Matrix 3D, row ordered
typedef GREAL mat3f[3][3];
//! Matrix 4D, row ordered
typedef GREAL mat4f[4][4];

//! Quaternion
typedef GREAL quatf[4];

//! Axis aligned box
struct aabb3f{
    GREAL minX;
    GREAL maxX;
    GREAL minY;
    GREAL maxY;
    GREAL minZ;
    GREAL maxZ;
};
//typedef struct _aabb3f aabb3f;
//! @}


/*! \defgroup VECTOR_OPERATIONS
Operations for vectors : vec2f,vec3f and vec4f
*/
//! @{

//! Zero out a 2D vector
#define VEC_ZERO_2(a)				\
{						\
   (a)[0] = (a)[1] = 0.0f;			\
}\


//! Zero out a 3D vector
#define VEC_ZERO(a)				\
{						\
   (a)[0] = (a)[1] = (a)[2] = 0.0f;		\
}\


/// Zero out a 4D vector
#define VEC_ZERO_4(a)				\
{						\
   (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f;	\
}\


/// Vector copy
#define VEC_COPY_2(b,a)				\
{						\
   (b)[0] = (a)[0];				\
   (b)[1] = (a)[1];				\
}\


/// Copy 3D vector
#define VEC_COPY(b,a)				\
{						\
   (b)[0] = (a)[0];				\
   (b)[1] = (a)[1];				\
   (b)[2] = (a)[2];				\
}\


/// Copy 4D vector
#define VEC_COPY_4(b,a)				\
{						\
   (b)[0] = (a)[0];				\
   (b)[1] = (a)[1];				\
   (b)[2] = (a)[2];				\
   (b)[3] = (a)[3];				\
}\


/// Vector difference
#define VEC_DIFF_2(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] - (v1)[0];		\
   (v21)[1] = (v2)[1] - (v1)[1];		\
}\


/// Vector difference
#define VEC_DIFF(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] - (v1)[0];		\
   (v21)[1] = (v2)[1] - (v1)[1];		\
   (v21)[2] = (v2)[2] - (v1)[2];		\
}\


/// Vector difference
#define VEC_DIFF_4(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] - (v1)[0];		\
   (v21)[1] = (v2)[1] - (v1)[1];		\
   (v21)[2] = (v2)[2] - (v1)[2];		\
   (v21)[3] = (v2)[3] - (v1)[3];		\
}\


/// Vector sum
#define VEC_SUM_2(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] + (v1)[0];		\
   (v21)[1] = (v2)[1] + (v1)[1];		\
}\


/// Vector sum
#define VEC_SUM(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] + (v1)[0];		\
   (v21)[1] = (v2)[1] + (v1)[1];		\
   (v21)[2] = (v2)[2] + (v1)[2];		\
}\


/// Vector sum
#define VEC_SUM_4(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] + (v1)[0];		\
   (v21)[1] = (v2)[1] + (v1)[1];		\
   (v21)[2] = (v2)[2] + (v1)[2];		\
   (v21)[3] = (v2)[3] + (v1)[3];		\
}\


/// scalar times vector
#define VEC_SCALE_2(c,a,b)			\
{						\
   (c)[0] = (a)*(b)[0];				\
   (c)[1] = (a)*(b)[1];				\
}\


/// scalar times vector
#define VEC_SCALE(c,a,b)			\
{						\
   (c)[0] = (a)*(b)[0];				\
   (c)[1] = (a)*(b)[1];				\
   (c)[2] = (a)*(b)[2];				\
}\


/// scalar times vector
#define VEC_SCALE_4(c,a,b)			\
{						\
   (c)[0] = (a)*(b)[0];				\
   (c)[1] = (a)*(b)[1];				\
   (c)[2] = (a)*(b)[2];				\
   (c)[3] = (a)*(b)[3];				\
}\


/// accumulate scaled vector
#define VEC_ACCUM_2(c,a,b)			\
{						\
   (c)[0] += (a)*(b)[0];			\
   (c)[1] += (a)*(b)[1];			\
}\


/// accumulate scaled vector
#define VEC_ACCUM(c,a,b)			\
{						\
   (c)[0] += (a)*(b)[0];			\
   (c)[1] += (a)*(b)[1];			\
   (c)[2] += (a)*(b)[2];			\
}\


/// accumulate scaled vector
#define VEC_ACCUM_4(c,a,b)			\
{						\
   (c)[0] += (a)*(b)[0];			\
   (c)[1] += (a)*(b)[1];			\
   (c)[2] += (a)*(b)[2];			\
   (c)[3] += (a)*(b)[3];			\
}\


/// Vector dot product
#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])


/// Vector dot product
#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])

/// Vector dot product
#define VEC_DOT_4(a,b)	((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])

/// vector impact parameter (squared)
#define VEC_IMPACT_SQ(bsq,direction,position) {\
   GREAL _llel_ = VEC_DOT(direction, position);\
   bsq = VEC_DOT(position, position) - _llel_*_llel_;\
}\


/// vector impact parameter
#define VEC_IMPACT(bsq,direction,position)	{\
   VEC_IMPACT_SQ(bsq,direction,position);		\
   GIM_SQRT(bsq,bsq);					\
}\

/// Vector length
#define VEC_LENGTH_2(a,l)\
{\
    GREAL _pp = VEC_DOT_2(a,a);\
    GIM_SQRT(_pp,l);\
}\


/// Vector length
#define VEC_LENGTH(a,l)\
{\
    GREAL _pp = VEC_DOT(a,a);\
    GIM_SQRT(_pp,l);\
}\


/// Vector length
#define VEC_LENGTH_4(a,l)\
{\
    GREAL _pp = VEC_DOT_4(a,a);\
    GIM_SQRT(_pp,l);\
}\

/// Vector inv length
#define VEC_INV_LENGTH_2(a,l)\
{\
    GREAL _pp = VEC_DOT_2(a,a);\
    GIM_INV_SQRT(_pp,l);\
}\


/// Vector inv length
#define VEC_INV_LENGTH(a,l)\
{\
    GREAL _pp = VEC_DOT(a,a);\
    GIM_INV_SQRT(_pp,l);\
}\


/// Vector inv length
#define VEC_INV_LENGTH_4(a,l)\
{\
    GREAL _pp = VEC_DOT_4(a,a);\
    GIM_INV_SQRT(_pp,l);\
}\



/// distance between two points
#define VEC_DISTANCE(_len,_va,_vb) {\
    vec3f _tmp_;				\
    VEC_DIFF(_tmp_, _vb, _va);			\
    VEC_LENGTH(_tmp_,_len);			\
}\


/// Vector length
#define VEC_CONJUGATE_LENGTH(a,l)\
{\
    GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
    GIM_SQRT(_pp,l);\
}\


/// Vector length
#define VEC_NORMALIZE(a) {	\
    GREAL len;\
    VEC_INV_LENGTH(a,len); \
    if(len<G_REAL_INFINITY)\
    {\
        a[0] *= len;				\
        a[1] *= len;				\
        a[2] *= len;				\
    }						\
}\

/// Set Vector size
#define VEC_RENORMALIZE(a,newlen) {	\
    GREAL len;\
    VEC_INV_LENGTH(a,len); \
    if(len<G_REAL_INFINITY)\
    {\
        len *= newlen;\
        a[0] *= len;				\
        a[1] *= len;				\
        a[2] *= len;				\
    }						\
}\

/// Vector cross
#define VEC_CROSS(c,a,b)		\
{						\
   c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];	\
   c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];	\
   c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];	\
}\


/*! Vector perp -- assumes that n is of unit length
 * accepts vector v, subtracts out any component parallel to n */
#define VEC_PERPENDICULAR(vp,v,n)			\
{						\
   GREAL dot = VEC_DOT(v, n);			\
   vp[0] = (v)[0] - dot*(n)[0];		\
   vp[1] = (v)[1] - dot*(n)[1];		\
   vp[2] = (v)[2] - dot*(n)[2];		\
}\


/*! Vector parallel -- assumes that n is of unit length */
#define VEC_PARALLEL(vp,v,n)			\
{						\
   GREAL dot = VEC_DOT(v, n);			\
   vp[0] = (dot) * (n)[0];			\
   vp[1] = (dot) * (n)[1];			\
   vp[2] = (dot) * (n)[2];			\
}\

/*! Same as Vector parallel --  n can have any length
 * accepts vector v, subtracts out any component perpendicular to n */
#define VEC_PROJECT(vp,v,n)			\
{ \
	GREAL scalar = VEC_DOT(v, n);			\
	scalar/= VEC_DOT(n, n); \
	vp[0] = (scalar) * (n)[0];			\
    vp[1] = (scalar) * (n)[1];			\
    vp[2] = (scalar) * (n)[2];			\
}\


/*! accepts vector v*/
#define VEC_UNPROJECT(vp,v,n)			\
{ \
	GREAL scalar = VEC_DOT(v, n);			\
	scalar = VEC_DOT(n, n)/scalar; \
	vp[0] = (scalar) * (n)[0];			\
    vp[1] = (scalar) * (n)[1];			\
    vp[2] = (scalar) * (n)[2];			\
}\


/*! Vector reflection -- assumes n is of unit length
 Takes vector v, reflects it against reflector n, and returns vr */
#define VEC_REFLECT(vr,v,n)			\
{						\
   GREAL dot = VEC_DOT(v, n);			\
   vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];	\
   vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];	\
   vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];	\
}\


/*! Vector blending
Takes two vectors a, b, blends them together with two scalars */
#define VEC_BLEND_AB(vr,sa,a,sb,b)			\
{						\
   vr[0] = (sa) * (a)[0] + (sb) * (b)[0];	\
   vr[1] = (sa) * (a)[1] + (sb) * (b)[1];	\
   vr[2] = (sa) * (a)[2] + (sb) * (b)[2];	\
}\

/*! Vector blending
Takes two vectors a, b, blends them together with s <=1 */
#define VEC_BLEND(vr,a,b,s)	VEC_BLEND_AB(vr,1-s,a,sb,s)

#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
//! @}


/*! \defgroup MATRIX_OPERATIONS
Operations for matrices : mat2f, mat3f and mat4f
*/
//! @{

/// initialize matrix
#define IDENTIFY_MATRIX_3X3(m)			\
{						\
   m[0][0] = 1.0;				\
   m[0][1] = 0.0;				\
   m[0][2] = 0.0;				\
						\
   m[1][0] = 0.0;				\
   m[1][1] = 1.0;				\
   m[1][2] = 0.0;				\
						\
   m[2][0] = 0.0;				\
   m[2][1] = 0.0;				\
   m[2][2] = 1.0;				\
}\

/*! initialize matrix */
#define IDENTIFY_MATRIX_4X4(m)			\
{						\
   m[0][0] = 1.0;				\
   m[0][1] = 0.0;				\
   m[0][2] = 0.0;				\
   m[0][3] = 0.0;				\
						\
   m[1][0] = 0.0;				\
   m[1][1] = 1.0;				\
   m[1][2] = 0.0;				\
   m[1][3] = 0.0;				\
						\
   m[2][0] = 0.0;				\
   m[2][1] = 0.0;				\
   m[2][2] = 1.0;				\