#ifndef FLOAT_MATH_LIB_H
#define FLOAT_MATH_LIB_H
//--------------------------------------------
// .CPP
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:[email protected]
**
** Portions of this source has been released with the PhysXViewer application, as
well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through
switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: [email protected]
**
**
** The MIT license:
**
** Permission is hereby granted, MEMALLOC_FREE of charge, to any person obtaining a
copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "FloatMath.h"
#define REAL float
#include "FloatMath.inl"
#undef REAL
#define REAL double
#include "FloatMath.inl"
//--------------------------------------
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:[email protected]
**
** The MIT license:
**
** Permission is hereby granted, free of charge, to any person obtaining a copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// a set of routines that let you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
//
// a vector (or point) is a 'float *' to 3 floating point numbers.
// a matrix is a 'float *' to an array of 16 floating point numbers representing a
4x4 transformation matrix compatible with D3D or OGL
// a quaternion is a 'float *' to 4 floats representing a quaternion x,y,z,w
//
//
//
// Please email bug fixes or improvements to John W. Ratcliff at
mailto:
[email protected]//
// If you find this source code useful donate a couple of bucks to my kid's fund
raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
#include <float.h>
enum FM_ClipState
{
FMCS_XMIN = (1<<0),
FMCS_XMAX = (1<<1),
FMCS_YMIN = (1<<2),
FMCS_YMAX = (1<<3),
FMCS_ZMIN = (1<<4),
FMCS_ZMAX = (1<<5),
};
enum FM_Axis
{
FM_XAXIS = (1<<0),
FM_YAXIS = (1<<1),
FM_ZAXIS = (1<<2)
};
enum LineSegmentType
{
LS_START,
LS_MIDDLE,
LS_END
};
const float FM_PI = 3.1415926535897932384626433832795028841971693993751f;
const float FM_DEG_TO_RAD = ((2.0f * FM_PI) / 360.0f);
const float FM_RAD_TO_DEG = (360.0f / (2.0f * FM_PI));
//***************** Float versions
//***
//*** vectors are assumed to be 3 floats or 3 doubles representing X, Y, Z
//*** quaternions are assumed to be 4 floats or 4 doubles representing X,Y,Z,W
//*** matrices are assumed to be 16 floats or 16 doubles representing a standard
D3D or OpenGL style 4x4 matrix
//*** bounding volumes are expressed as two sets of 3 floats/double representing
bmin(x,y,z) and bmax(x,y,z)
//*** Plane equations are assumed to be 4 floats or 4 doubles representing
Ax,By,Cz,D
FM_Axis fm_getDominantAxis(const float normal[3]);
FM_Axis fm_getDominantAxis(const double normal[3]);
void fm_decomposeTransform(const float local_transform[16],float trans[3],float
rot[4],float scale[3]);
void fm_decomposeTransform(const double local_transform[16],double trans[3],double
rot[4],double scale[3]);
void fm_multiplyTransform(const float *pA,const float *pB,float *pM);
void fm_multiplyTransform(const double *pA,const double *pB,double *pM);
void fm_inverseTransform(const float matrix[16],float inverse_matrix[16]);
void fm_inverseTransform(const double matrix[16],double inverse_matrix[16]);
void fm_identity(float matrix[16]); // set 4x4 matrix to identity.
void fm_identity(double matrix[16]); // set 4x4 matrix to identity.
void fm_inverseRT(const float matrix[16], const float pos[3], float t[3]); //
inverse rotate translate the point.
void fm_inverseRT(const double matrix[16],const double pos[3],double t[3]); //
inverse rotate translate the point.
void fm_transform(const float matrix[16], const float pos[3], float t[3]); //
rotate and translate this point.
void fm_transform(const double matrix[16],const double pos[3],double t[3]); //
rotate and translate this point.
float fm_getDeterminant(const float matrix[16]);
double fm_getDeterminant(const double matrix[16]);
void fm_getSubMatrix(int ki,int kj,float pDst[16],const float matrix[16]);
void fm_getSubMatrix(int ki,int kj,double pDst[16],const float matrix[16]);
void fm_rotate(const float matrix[16],const float pos[3],float t[3]); // only
rotate the point by a 4x4 matrix, don't translate.
void fm_rotate(const double matri[16],const double pos[3],double t[3]); // only
rotate the point by a 4x4 matrix, don't translate.
void fm_eulerToMatrix(float ax,float ay,float az,float matrix[16]); // convert
euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_eulerToMatrix(double ax,double ay,double az,double matrix[16]); // convert
euler (in radians) to a dest 4x4 matrix (translation set to zero)
void fm_getAABB(size_t vcount,const float *points,size_t pstride,float
bmin[3],float bmax[3]);
void fm_getAABB(size_t vcount,const double *points,size_t pstride,double
bmin[3],double bmax[3]);
void fm_getAABBCenter(const float bmin[3],const float bmax[3],float center[3]);
void fm_getAABBCenter(const double bmin[3],const double bmax[3],double center[3]);
void fm_eulerToQuat(float x,float y,float z,float quat[4]); // convert euler
angles to quaternion.
void fm_eulerToQuat(double x,double y,double z,double quat[4]); // convert euler
angles to quaternion.
void fm_quatToEuler(const float quat[4],float &ax,float &ay,float &az);
void fm_quatToEuler(const double quat[4],double &ax,double &ay,double &az);
void fm_eulerToQuat(const float euler[3],float quat[4]); // convert euler angles
to quaternion. Angles must be radians not degrees!
void fm_eulerToQuat(const double euler[3],double quat[4]); // convert euler angles
to quaternion.
void fm_scale(float x,float y,float z,float matrix[16]); // apply scale to the
matrix.
void fm_scale(double x,double y,double z,double matrix[16]); // apply scale to the
matrix.
void fm_eulerToQuatDX(float x,float y,float z,float quat[4]); // convert euler
angles to quaternion using the fucked up DirectX method
void fm_eulerToQuatDX(double x,double y,double z,double quat[4]); // convert euler
angles to quaternion using the fucked up DirectX method
void fm_eulerToMatrixDX(float x,float y,float z,float matrix[16]); // convert
euler angles to quaternion using the fucked up DirectX method.
void fm_eulerToMatrixDX(double x,double y,double z,double matrix[16]); // convert
euler angles to quaternion using the fucked up DirectX method.
void fm_quatToMatrix(const float quat[4],float matrix[16]); // convert quaterinion
rotation to matrix, translation set to zero.
void fm_quatToMatrix(const double quat[4],double matrix[16]); // convert
quaterinion rotation to matrix, translation set to zero.
void fm_quatRotate(const float quat[4],const float v[3],float r[3]); // rotate a
vector directly by a quaternion.
void fm_quatRotate(const double quat[4],const double v[3],double r[3]); // rotate
a vector directly by a quaternion.
void fm_getTranslation(const float matrix[16],float t[3]);
void fm_getTranslation(const double matrix[16],double t[3]);
void fm_setTranslation(const float *translation,float matrix[16]);
void fm_setTranslation(const double *translation,double matrix[16]);
void fm_matrixToQuat(const float matrix[16],float quat[4]); // convert the 3x3
portion of a 4x4 matrix into a quaterion as x,y,z,w
void fm_matrixToQuat(const double matrix[16],double quat[4]); // convert the 3x3
portion of a 4x4 matrix into a quaterion as x,y,z,w
float fm_sphereVolume(float radius); // return's the volume of a sphere of this
radius (4/3 PI * R cubed )
double fm_sphereVolume(double radius); // return's the volume of a sphere of this
radius (4/3 PI * R cubed )
float fm_cylinderVolume(float radius,float h);
double fm_cylinderVolume(double radius,double h);
float fm_capsuleVolume(float radius,float h);
double fm_capsuleVolume(double radius,double h);
float fm_distance(const float p1[3],const float p2[3]);
double fm_distance(const double p1[3],const double p2[3]);
float fm_distanceSquared(const float p1[3],const float p2[3]);
double fm_distanceSquared(const double p1[3],const double p2[3]);
float fm_distanceSquaredXZ(const float p1[3],const float p2[3]);
double fm_distanceSquaredXZ(const double p1[3],const double p2[3]);
float fm_computePlane(const float p1[3],const float p2[3],const float p3[3],float
*n); // return D
double fm_computePlane(const double p1[3],const double p2[3],const double
p3[3],double *n); // return D
float fm_distToPlane(const float plane[4],const float pos[3]); // computes the
distance of this point from the plane.
double fm_distToPlane(const double plane[4],const double pos[3]); // computes the
distance of this point from the plane.
float fm_dot(const float p1[3],const float p2[3]);
double fm_dot(const double p1[3],const double p2[3]);
void fm_cross(float cross[3],const float a[3],const float b[3]);
void fm_cross(double cross[3],const double a[3],const double b[3]);
void fm_computeNormalVector(float n[3],const float p1[3],const float p2[3]); // as
P2-P1 normalized.
void fm_computeNormalVector(double n[3],const double p1[3],const double p2[3]); //
as P2-P1 normalized.
bool fm_computeWindingOrder(const float p1[3],const float p2[3],const float
p3[3]); // returns true if the triangle is clockwise.
bool fm_computeWindingOrder(const double p1[3],const double p2[3],const double
p3[3]); // returns true if the triangle is clockwise.
float fm_normalize(float n[3]); // normalize this vector and return the distance
double fm_normalize(double n[3]); // normalize this vector and return the distance
void fm_matrixMultiply(const float A[16],const float B[16],float dest[16]);
void fm_matrixMultiply(const double A[16],const double B[16],double dest[16]);
void fm_composeTransform(const float position[3],const float quat[4],const float
scale[3],float matrix[16]);
void fm_composeTransform(const double position[3],const double quat[4],const
double scale[3],double matrix[16]);
float fm_computeArea(const float p1[3],const float p2[3],const float p3[3]);
double fm_computeArea(const double p1[3],const double p2[3],const double p3[3]);
void fm_lerp(const float p1[3],const float p2[3],float dest[3],float lerpValue);
void fm_lerp(const double p1[3],const double p2[3],double dest[3],double
lerpValue);
bool fm_insideTriangleXZ(const float test[3],const float p1[3],const float
p2[3],const float p3[3]);
bool fm_insideTriangleXZ(const double test[3],const double p1[3],const double
p2[3],const double p3[3]);
bool fm_insideAABB(const float pos[3],const float bmin[3],const float bmax[3]);
bool fm_insideAABB(const double pos[3],const double bmin[3],const double bmax[3]);
bool fm_insideAABB(const float obmin[3],const float obmax[3],const float
tbmin[3],const float tbmax[3]); // test if bounding box tbmin/tmbax is fully inside
obmin/obmax
bool fm_insideAABB(const double obmin[3],const double obmax[3],const double
tbmin[3],const double tbmax[3]); // test if bounding box tbmin/tmbax is fully
inside obmin/obmax
size_t fm_clipTestPoint(const float bmin[3],const float bmax[3],const float
pos[3]);
size_t fm_clipTestPoint(const double bmin[3],const double bmax[3],const double
pos[3]);
size_t fm_clipTestPointXZ(const float bmin[3],const float bmax[3],const float
pos[3]); // only tests X and Z, not Y
size_t fm_clipTestPointXZ(const double bmin[3],const double bmax[3],const double
pos[3]); // only tests X and Z, not Y
size_t fm_clipTestAABB(const float bmin[3],const float bmax[3],const float
p1[3],const float p2[3],const float p3[3],size_t &andCode);
size_t fm_clipTestAABB(const double bmin[3],const double bmax[3],const double
p1[3],const double p2[3],const double p3[3],size_t &andCode);
bool fm_lineTestAABBXZ(const float p1[3],const float p2[3],const float
bmin[3],const float bmax[3],float &time);
bool fm_lineTestAABBXZ(const double p1[3],const double p2[3],const double
bmin[3],const double bmax[3],double &time);
bool fm_lineTestAABB(const float p1[3],const float p2[3],const float
bmin[3],const float bmax[3],float &time);
bool fm_lineTestAABB(const double p1[3],const double p2[3],const double
bmin[3],const double bmax[3],double &time);
void fm_initMinMax(const float p[3],float bmin[3],float bmax[3]);
void fm_initMinMax(const double p[3],double bmin[3],double bmax[3]);
void fm_initMinMax(float bmin[3],float bmax[3]);
void fm_initMinMax(double bmin[3],double bmax[3]);
void fm_minmax(const float p[3],float bmin[3],float bmax[3]); // accmulate to a
min-max value
void fm_minmax(const double p[3],double bmin[3],double bmax[3]); // accmulate to a
min-max value
float fm_solveX(const float plane[4],float y,float z); // solve for X given this
plane equation and the other two components.
double fm_solveX(const double plane[4],double y,double z); // solve for X given
this plane equation and the other two components.
float fm_solveY(const float plane[4],float x,float z); // solve for Y given this
plane equation and the other two components.
double fm_solveY(const double plane[4],double x,double z); // solve for Y given
this plane equation and the other two components.
float fm_solveZ(const float plane[4],float x,float y); // solve for Z given this
plane equation and the other two components.
double fm_solveZ(const double plane[4],double x,double y); // solve for Z given
this plane equation and the other two components.
bool fm_computeBestFitPlane(size_t vcount, // number of input data points
const float *points, // starting address of points array.
size_t vstride, // stride between input points.
const float *weights, // *optional point weighting values.
size_t wstride, // weight stride for each vertex.
float plane[4]);
bool fm_computeBestFitPlane(size_t vcount, // number of input data points
const double *points, // starting address of points array.
size_t vstride, // stride between input points.
const double *weights, // *optional point weighting values.
size_t wstride, // weight stride for each vertex.
double plane[4]);
float fm_computeBestFitAABB(size_t vcount,const float *points,size_t pstride,float
bmin[3],float bmax[3]); // returns the diagonal distance
double fm_computeBestFitAABB(size_t vcount,const double *points,size_t
pstride,double bmin[3],double bmax[3]); // returns the diagonal distance
float fm_computeBestFitSphere(size_t vcount,const float *points,size_t
pstride,float center[3]);
double fm_computeBestFitSphere(size_t vcount,const double *points,size_t
pstride,double center[3]);
bool fm_lineSphereIntersect(const float center[3],float radius,const float
p1[3],const float p2[3],float intersect[3]);
bool fm_lineSphereIntersect(const double center[3],double radius,const double
p1[3],const double p2[3],double intersect[3]);
bool fm_intersectRayAABB(const float bmin[3],const float bmax[3],const float
pos[3],const float dir[3],float intersect[3]);
bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const
float p1[3],const float p2[3],float intersect[3]);
bool fm_lineIntersectsTriangle(const float rayStart[3],const float rayEnd[3],const
float p1[3],const float p2[3],const float p3[3],float sect[3]);
bool fm_lineIntersectsTriangle(const double rayStart[3],const double
rayEnd[3],const double p1[3],const double p2[3],const double p3[3],double sect[3]);
bool fm_rayIntersectsTriangle(const float origin[3],const float dir[3],const float
v0[3],const float v1[3],const float v2[3],float &t);
bool fm_rayIntersectsTriangle(const double origin[3],const double dir[3],const
double v0[3],const double v1[3],const double v2[3],double &t);
bool fm_raySphereIntersect(const float center[3],float radius,const float
pos[3],const float dir[3],float distance,float intersect[3]);
bool fm_raySphereIntersect(const double center[3],double radius,const double
pos[3],const double dir[3],double distance,double intersect[3]);
void fm_catmullRom(float out_vector[3],const float p1[3],const float p2[3],const
float p3[3],const float *p4, const float s);
void fm_catmullRom(double out_vector[3],const double p1[3],const double p2[3],const
double p3[3],const double *p4, const double s);
bool fm_intersectAABB(const float bmin1[3],const float bmax1[3],const float
bmin2[3],const float bmax2[3]);
bool fm_intersectAABB(const double bmin1[3],const double bmax1[3],const double
bmin2[3],const double bmax2[3]);
// computes the rotation quaternion to go from unit-vector v0 to unit-vector v1
void fm_rotationArc(const float v0[3],const float v1[3],float quat[4]);
void fm_rotationArc(const double v0[3],const double v1[3],double quat[4]);
float fm_distancePointLineSegment(const float Point[3],const float
LineStart[3],const float LineEnd[3],float intersection[3],LineSegmentType &type);
double fm_distancePointLineSegment(const double Point[3],const double
LineStart[3],const double LineEnd[3],double intersection[3],LineSegmentType &type);
bool fm_colinear(const double p1[3],const double p2[3],const double p3[3],double
epsilon=0.999); // true if these three points in a row are co-linear
bool fm_colinear(const float p1[3],const float p2[3],const float p3[3],float
epsilon=0.999f);
bool fm_colinear(const float a1[3],const float a2[3],const float b1[3],const float
b2[3]); // true if these two line segments are co-linear.
bool fm_colinear(const double a1[3],const double a2[3],const double b1[3],const
double b2[3]); // true if these two line segments are co-linear.
enum IntersectResult
{
IR_DONT_INTERSECT,
IR_DO_INTERSECT,
IR_COINCIDENT,
IR_PARALLEL,
};
IntersectResult fm_intersectLineSegments2d(const float a1[3], const float a2[3],
const float b1[3], const float b2[3], float intersectionPoint[3]);
IntersectResult fm_intersectLineSegments2d(const double a1[3],const double
a2[3],const double b1[3],const double b2[3],double intersectionPoint[3]);
IntersectResult fm_intersectLineSegments2dTime(const float a1[3], const float
a2[3], const float b1[3], const float b2[3],float &t1,float &t2);
IntersectResult fm_intersectLineSegments2dTime(const double a1[3],const double
a2[3],const double b1[3],const double b2[3],double &t1,double &t2);
// Plane-Triangle splitting
enum PlaneTriResult
{
PTR_ON_PLANE,
PTR_FRONT,
PTR_BACK,
PTR_SPLIT,
};
PlaneTriResult fm_planeTriIntersection(const float plane[4], // the plane
equation in Ax+By+Cz+D format
const float *triangle, // the source triangle.
size_t tstride, // stride in bytes of the
input and output *vertices*
float epsilon, // the co-planer epsilon
value.
float *front, // the triangle in front
of the
size_t &fcount, // number of vertices in the
'front' triangle
float *back, // the triangle in back
of the plane
size_t &bcount); // the number of vertices in
the 'back' triangle.
PlaneTriResult fm_planeTriIntersection(const double plane[4], // the plane
equation in Ax+By+Cz+D format
const double *triangle, // the source triangle.
size_t tstride, // stride in bytes of the
input and output *vertices*
double epsilon, // the co-planer
epsilon value.
double *front, // the triangle in
front of the
size_t &fcount, // number of vertices in the
'front' triangle
double *back, // the triangle in back
of the plane
size_t &bcount); // the number of vertices in
the 'back' triangle.
void fm_intersectPointPlane(const float p1[3],const float p2[3],float *split,const
float plane[4]);
void fm_intersectPointPlane(const double p1[3],const double p2[3],double
*split,const double plane[4]);
PlaneTriResult fm_getSidePlane(const float p[3],const float plane[4],float
epsilon);
PlaneTriResult fm_getSidePlane(const double p[3],const double plane[4],double
epsilon);
enum FitStrategy
{
FS_FAST_FIT, // just computes the diagonals only, can be off substantially at
times.
FS_MEDIUM_FIT, // rotates on one axis to converge to a solution.
FS_SLOW_FIT, // rotates on all three axes to find the best fit.
};
void fm_computeBestFitOBB(size_t vcount,const float *points,size_t pstride,float
*sides,float matrix[16],FitStrategy strategy=FS_MEDIUM_FIT);
void fm_computeBestFitOBB(size_t vcount,const double *points,size_t pstride,double
*sides,double matrix[16],FitStrategy strategy=FS_MEDIUM_FIT);
void fm_computeBestFitOBB(size_t vcount,const float *points,size_t pstride,float
*sides,float pos[3],float quat[4],FitStrategy strategy=FS_MEDIUM_FIT);
void fm_computeBestFitOBB(size_t vcount,const double *points,size_t pstride,double
*sides,double pos[3],double quat[4],FitStrategy strategy=FS_MEDIUM_FIT);
void fm_computeBestFitABB(size_t vcount,const float *points,size_t pstride,float
*sides,float pos[3]);
void fm_computeBestFitABB(size_t vcount,const double *points,size_t pstride,double
*sides,double pos[3]);
//** Note, if the returned capsule height is less than zero, then you must
represent it is a sphere of size radius.
void fm_computeBestFitCapsule(size_t vcount,const float *points,size_t
pstride,float &radius,float &height,float matrix[16],FitStrategy
strategy=FS_MEDIUM_FIT);
void fm_computeBestFitCapsule(size_t vcount,const double *points,size_t
pstride,float &radius,float &height,double matrix[16],FitStrategy
strategy=FS_MEDIUM_FIT);
void fm_planeToMatrix(const float plane[4],float matrix[16]); // convert a plane
equation to a 4x4 rotation matrix. Reference vector is 0,1,0
void fm_planeToQuat(const float plane[4],float quat[4],float pos[3]); // convert a
plane equation to a quaternion and translation
void fm_planeToMatrix(const double plane[4],double matrix[16]); // convert a plane
equation to a 4x4 rotation matrix
void fm_planeToQuat(const double plane[4],double quat[4],double pos[3]); // convert
a plane equation to a quaternion and translation
inline void fm_doubleToFloat3(const double p[3],float t[3]) { t[0] = (float) p[0];
t[1] = (float)p[1]; t[2] = (float)p[2]; };
inline void fm_floatToDouble3(const float p[3],double t[3]) { t[0] = (double)p[0];
t[1] = (double)p[1]; t[2] = (double)p[2]; };
void fm_eulerMatrix(float ax,float ay,float az,float matrix[16]); // convert euler
(in radians) to a dest 4x4 matrix (translation set to zero)
void fm_eulerMatrix(double ax,double ay,double az,double matrix[16]); // convert
euler (in radians) to a dest 4x4 matrix (translation set to zero)
float fm_computeMeshVolume(const float *vertices,size_t tcount,const unsigned int
*indices);
double fm_computeMeshVolume(const double *vertices,size_t tcount,const unsigned int
*indices);
#define FM_DEFAULT_GRANULARITY 0.001f // 1 millimeter is the default granularity
class fm_VertexIndex
{
public:
virtual size_t getIndex(const float pos[3],bool &newPos) = 0; // get
welded index for this float vector[3]
virtual size_t getIndex(const double pos[3],bool &newPos) = 0; // get
welded index for this double vector[3]
virtual const float * getVerticesFloat(void) const = 0;
virtual const double * getVerticesDouble(void) const = 0;
virtual const float * getVertexFloat(size_t index) const = 0;
virtual const double * getVertexDouble(size_t index) const = 0;
virtual size_t getVcount(void) const = 0;
virtual bool isDouble(void) const = 0;
virtual bool saveAsObj(const char *fname,unsigned int tcount,unsigned
int *indices) = 0;
};
fm_VertexIndex * fm_createVertexIndex(double granularity,bool snapToGrid); //
create an indexed vertex system for doubles
fm_VertexIndex * fm_createVertexIndex(float granularity,bool snapToGrid); //
create an indexed vertext system for floats
void fm_releaseVertexIndex(fm_VertexIndex *vindex);
class fm_LineSegment
{
public:
fm_LineSegment(void)
{
mE1 = mE2 = 0;
}
fm_LineSegment(size_t e1,size_t e2)
{
mE1 = e1;
mE2 = e2;
}
size_t mE1;
size_t mE2;
};
// LineSweep *only* supports doublees. As a geometric operation it needs as much
precision as possible.
class fm_LineSweep
{
public:
virtual fm_LineSegment * performLineSweep(const fm_LineSegment *segments,
size_t icount,
const double *planeEquation,
fm_VertexIndex *pool,
size_t &scount) = 0;
};
fm_LineSweep * fm_createLineSweep(void);
void fm_releaseLineSweep(fm_LineSweep *sweep);
class fm_Triangulate
{
public:
virtual const double * triangulate3d(size_t pcount,const double
*points,size_t vstride,size_t &tcount) = 0;
virtual const float * triangulate3d(size_t pcount,const float
*points,size_t vstride,size_t &tcount) = 0;
};
fm_Triangulate * fm_createTriangulate(void);
void fm_releaseTriangulate(fm_Triangulate *t);
const float * fm_getPoint(const float *points,size_t pstride,size_t index);
const double * fm_getPoint(const double *points,size_t pstride,size_t index);
bool fm_insideTriangle(float Ax, float Ay,float Bx, float By,float Cx, float
Cy,float Px, float Py);
bool fm_insideTriangle(double Ax, double Ay,double Bx, double By,double Cx,
double Cy,double Px, double Py);
float fm_areaPolygon2d(size_t pcount,const float *points,size_t pstride);
double fm_areaPolygon2d(size_t pcount,const double *points,size_t pstride);
bool fm_pointInsidePolygon2d(size_t pcount,const float *points,size_t
pstride,const float *point,size_t xindex=0,size_t yindex=1);
bool fm_pointInsidePolygon2d(size_t pcount,const double *points,size_t
pstride,const double *point,size_t xindex=0,size_t yindex=1);
size_t fm_consolidatePolygon(size_t pcount,const float *points,size_t pstride,float
*dest,float epsilon=0.999f); // collapses co-linear edges.
size_t fm_consolidatePolygon(size_t pcount,const double *points,size_t
pstride,double *dest,double epsilon=0.999); // collapses co-linear edges.
bool fm_computeSplitPlane(unsigned int vcount,const double *vertices,unsigned int
tcount,const unsigned int *indices,double *plane);
bool fm_computeSplitPlane(unsigned int vcount,const float *vertices,unsigned int
tcount,const unsigned int *indices,float *plane);
void fm_nearestPointInTriangle(const float *pos,const float *p1,const float
*p2,const float *p3,float *nearest);
void fm_nearestPointInTriangle(const double *pos,const double *p1,const double
*p2,const double *p3,double *nearest);
float fm_areaTriangle(const float *p1,const float *p2,const float *p3);
double fm_areaTriangle(const double *p1,const double *p2,const double *p3);
void fm_subtract(const float *A,const float *B,float *diff); // compute A-B and
store the result in 'diff'
void fm_subtract(const double *A,const double *B,double *diff); // compute A-B and
store the result in 'diff'
void fm_multiply(float *A,float scaler);
void fm_multiply(double *A,double scaler);
void fm_add(const float *A,const float *B,float *sum);
void fm_add(const double *A,const double *B,double *sum);
void fm_copy3(const float *source,float *dest);
void fm_copy3(const double *source,double *dest);
// re-indexes an indexed triangle mesh but drops unused vertices. The
output_indices can be the same pointer as the input indices.
// the output_vertices can point to the input vertices if you desire. The
output_vertices buffer should be at least the same size
// is the input buffer. The routine returns the new vertex count after re-
indexing.
size_t fm_copyUniqueVertices(size_t vcount,const float *input_vertices,float
*output_vertices,size_t tcount,const size_t *input_indices,size_t *output_indices);
size_t fm_copyUniqueVertices(size_t vcount,const double *input_vertices,double
*output_vertices,size_t tcount,const size_t *input_indices,size_t *output_indices);
bool fm_isMeshCoplanar(size_t tcount,const size_t *indices,const float
*vertices,bool doubleSided); // returns true if this collection of indexed
triangles are co-planar!
bool fm_isMeshCoplanar(size_t tcount,const size_t *indices,const double
*vertices,bool doubleSided); // returns true if this collection of indexed
triangles are co-planar!
bool fm_samePlane(const float p1[4],const float p2[4],float
normalEpsilon=0.01f,float dEpsilon=0.001f,bool doubleSided=false); // returns true
if these two plane equations are identical within an epsilon
bool fm_samePlane(const double p1[4],const double p2[4],double
normalEpsilon=0.01,double dEpsilon=0.001,bool doubleSided=false);
void fm_OBBtoAABB(const float obmin[3],const float obmax[3],const float
matrix[16],float abmin[3],float abmax[3]);
// a utility class that will tesseleate a mesh.
class fm_Tesselate
{
public:
virtual const size_t * tesselate(fm_VertexIndex *vindex,size_t tcount,const
size_t *indices,float longEdge,size_t maxDepth,size_t &outcount) = 0;
};
fm_Tesselate * fm_createTesselate(void);
void fm_releaseTesselate(fm_Tesselate *t);
void fm_computeMeanNormals(unsigned int vcount, // the number of vertices
const float *vertices, // the base address of the
vertex position data.
unsigned int vstride, // the stride between
position data.
float *normals, // the base address of the
destination for mean vector normals
unsigned int nstride, // the stride between normals
unsigned int tcount, // the number of triangles
const unsigned int *indices); // the triangle
indices
void fm_computeMeanNormals(unsigned int vcount, // the number of vertices
const double *vertices, // the base address of the
vertex position data.
unsigned int vstride, // the stride between
position data.
double *normals, // the base address of the
destination for mean vector normals
unsigned int nstride, // the stride between normals
unsigned int tcount, // the number of triangles
const unsigned int *indices); // the triangle
indices
#endif
//
===================================================================================
=====================
// .INL
/*!
**
** Copyright (c) 2007 by John W. Ratcliff mailto:[email protected]
**
** Portions of this source has been released with the PhysXViewer application, as
well as
** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
**
** If you find this code useful or you are feeling particularily generous I would
** ask that you please go to http://www.amillionpixels.us and make a donation
** to Troy DeMolay.
**
** DeMolay is a youth group for young men between the ages of 12 and 21.
** It teaches strong moral principles, as well as leadership skills and
** public speaking. The donations page uses the 'pay for pixels' paradigm
** where, in this case, a pixel is only a single penny. Donations can be
** made for as small as $4 or as high as a $100 block. Each person who donates
** will get a link to their own site as well as acknowledgement on the
** donations blog located here http://www.amillionpixels.blogspot.com/
**
** If you wish to contact me you can use the following methods:
**
** Skype Phone: 636-486-4040 (let it ring a long time while it goes through
switches)
** Skype ID: jratcliff63367
** Yahoo: jratcliff63367
** AOL: jratcliff1961
** email: [email protected]
**
**
** The MIT license:
**
** Permission is hereby granted, MEMALLOC_FREE of charge, to any person obtaining a
copy
** of this software and associated documentation files (the "Software"), to deal
** in the Software without restriction, including without limitation the rights
** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
** copies of the Software, and to permit persons to whom the Software is furnished
** to do so, subject to the following conditions:
**
** The above copyright notice and this permission notice shall be included in all
** copies or substantial portions of the Software.
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY,
** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// a set of routines that let you do common 3d math
// operations without any vector, matrix, or quaternion
// classes or templates.
//
// a vector (or point) is a 'float *' to 3 floating point numbers.
// a matrix is a 'float *' to an array of 16 floating point numbers representing a
4x4 transformation matrix compatible with D3D or OGL
// a quaternion is a 'float *' to 4 floats representing a quaternion x,y,z,w
//
//
//
// Please email bug fixes or improvements to John W. Ratcliff at
mailto:
[email protected]//
// If you find this source code useful donate a couple of bucks to my kid's fund
raising website at
// www.amillionpixels.us
//
// More snippets at: www.codesuppository.com
//
#pragma warning(disable:4996)
#include "UserMemAlloc.h"
void fm_inverseRT(const REAL matrix[16],const REAL pos[3],REAL t[3]) // inverse
rotate translate the point.
{
REAL _x = pos[0] - matrix[3*4+0];
REAL _y = pos[1] - matrix[3*4+1];
REAL _z = pos[2] - matrix[3*4+2];
// Multiply inverse-translated source vector by inverted rotation transform
t[0] = (matrix[0*4+0] * _x) + (matrix[0*4+1] * _y) + (matrix[0*4+2] * _z);
t[1] = (matrix[1*4+0] * _x) + (matrix[1*4+1] * _y) + (matrix[1*4+2] * _z);
t[2] = (matrix[2*4+0] * _x) + (matrix[2*4+1] * _y) + (matrix[2*4+2] * _z);
REAL fm_getDeterminant(const REAL matrix[16])
{
REAL tempv[3];
REAL p0[3];
REAL p1[3];
REAL p2[3];
p0[0] = matrix[0*4+0];
p0[1] = matrix[0*4+1];
p0[2] = matrix[0*4+2];
p1[0] = matrix[1*4+0];
p1[1] = matrix[1*4+1];
p1[2] = matrix[1*4+2];
p2[0] = matrix[2*4+0];
p2[1] = matrix[2*4+1];
p2[2] = matrix[2*4+2];
fm_cross(tempv,p1,p2);
return fm_dot(p0,tempv);
REAL fm_squared(REAL x) { return x*x; };
void fm_decomposeTransform(const REAL local_transform[16],REAL trans[3],REAL
rot[4],REAL scale[3])
{
trans[0] = local_transform[12];
trans[1] = local_transform[13];
trans[2] = local_transform[14];
scale[0] = sqrt(fm_squared(local_transform[0*4+0]) +
fm_squared(local_transform[0*4+1]) + fm_squared(local_transform[0*4+2]));
scale[1] = sqrt(fm_squared(local_transform[1*4+0]) +
fm_squared(local_transform[1*4+1]) + fm_squared(local_transform[1*4+2]));
scale[2] = sqrt(fm_squared(local_transform[2*4+0]) +
fm_squared(local_transform[2*4+1]) + fm_squared(local_transform[2*4+2]));
REAL m[16];
memcpy(m,local_transform,sizeof(REAL)*16);
REAL sx = 1.0f / scale[0];
REAL sy = 1.0f / scale[1];
REAL sz = 1.0f / scale[2];
m[0*4+0]*=sx;
m[0*4+1]*=sx;
m[0*4+2]*=sx;
m[1*4+0]*=sy;
m[1*4+1]*=sy;
m[1*4+2]*=sy;
m[2*4+0]*=sz;
m[2*4+1]*=sz;
m[2*4+2]*=sz;
fm_matrixToQuat(m,rot);
void fm_getSubMatrix(int ki,int kj,REAL pDst[16],const REAL matrix[16])
{
int row, col;
int dstCol = 0, dstRow = 0;
for ( col = 0; col < 4; col++ )
{
if ( col == kj )
{
continue;
}
for ( dstRow = 0, row = 0; row < 4; row++ )
{
if ( row == ki )
{
continue;
}
pDst[dstCol*4+dstRow] = matrix[col*4+row];
dstRow++;
}
dstCol++;
}
}
void fm_inverseTransform(const REAL matrix[16],REAL inverse_matrix[16])
{
REAL determinant = fm_getDeterminant(matrix);
determinant = 1.0f / determinant;
for (int i = 0; i < 4; i++ )
{
for (int j = 0; j < 4; j++ )
{
int sign = 1 - ( ( i + j ) % 2 ) * 2;
REAL subMat[16];
fm_identity(subMat);
fm_getSubMatrix( i, j, subMat, matrix );
REAL subDeterminant = fm_getDeterminant(subMat);
inverse_matrix[i*4+j] = ( subDeterminant * sign ) * determinant;
}
}
}
void fm_identity(REAL matrix[16]) // set 4x4 matrix to identity.
{
matrix[0*4+0] = 1;
matrix[1*4+1] = 1;
matrix[2*4+2] = 1;
matrix[3*4+3] = 1;
matrix[1*4+0] = 0;
matrix[2*4+0] = 0;
matrix[3*4+0] = 0;
matrix[0*4+1] = 0;
matrix[2*4+1] = 0;
matrix[3*4+1] = 0;
matrix[0*4+2] = 0;
matrix[1*4+2] = 0;
matrix[3*4+2] = 0;
matrix[0*4+3] = 0;
matrix[1*4+3] = 0;
matrix[2*4+3] = 0;
void fm_quatToEuler(const REAL quat[4],REAL &ax,REAL &ay,REAL &az)
{
REAL x = quat[0];
REAL y = quat[1];
REAL z = quat[2];
REAL w = quat[3];
REAL sint = (2.0f * w * y) - (2.0f * x * z);
REAL cost_temp = 1.0f - (sint * sint);
REAL cost = 0;
if ( (REAL)fabs(cost_temp) > 0.001f )
{
cost = sqrt( cost_temp );
}
REAL sinv, cosv, sinf, cosf;
if ( (REAL)fabs(cost) > 0.001f )
{
cost = 1.0f / cost;
sinv = ((2.0f * y * z) + (2.0f * w * x)) * cost;
cosv = (1.0f - (2.0f * x * x) - (2.0f * y * y)) * cost;
sinf = ((2.0f * x * y) + (2.0f * w * z)) * cost;
cosf = (1.0f - (2.0f * y * y) - (2.0f * z * z)) * cost;
}
else
{
sinv = (2.0f * w * x) - (2.0f * y * z);
cosv = 1.0f - (2.0f * x * x) - (2.0f * z * z);
sinf = 0;
cosf = 1.0f;
}
// compute output rotations
ax = atan2( sinv, cosv );
ay = atan2( sint, cost );
az = atan2( sinf, cosf );
void fm_eulerToMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in
radians) to a dest 4x4 matrix (translation set to zero)
{
REAL quat[4];
fm_eulerToQuat(ax,ay,az,quat);
fm_quatToMatrix(quat,matrix);
}
void fm_getAABB(size_t vcount,const REAL *points,size_t pstride,REAL *bmin,REAL
*bmax)
{
const unsigned char *source = (const unsigned char *) points;
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
for (size_t i=1; i<vcount; i++)
{
source+=pstride;
const REAL *p = (const REAL *) source;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
}
}
void fm_eulerToQuat(const REAL *euler,REAL *quat) // convert euler angles to
quaternion.
{
fm_eulerToQuat(euler[0],euler[1],euler[2],quat);
}
void fm_eulerToQuat(REAL roll,REAL pitch,REAL yaw,REAL *quat) // convert euler
angles to quaternion.
{
roll *= 0.5f;
pitch *= 0.5f;
yaw *= 0.5f;
REAL cr = cos(roll);
REAL cp = cos(pitch);
REAL cy = cos(yaw);
REAL sr = sin(roll);
REAL sp = sin(pitch);
REAL sy = sin(yaw);
REAL cpcy = cp * cy;
REAL spsy = sp * sy;
REAL spcy = sp * cy;
REAL cpsy = cp * sy;
quat[0] = ( sr * cpcy - cr * spsy);
quat[1] = ( cr * spcy + sr * cpsy);
quat[2] = ( cr * cpsy - sr * spcy);
quat[3] = cr * cpcy + sr * spsy;
}
void fm_quatToMatrix(const REAL *quat,REAL *matrix) // convert quaterinion rotation
to matrix, zeros out the translation component.
{
REAL xx = quat[0]*quat[0];
REAL yy = quat[1]*quat[1];
REAL zz = quat[2]*quat[2];
REAL xy = quat[0]*quat[1];
REAL xz = quat[0]*quat[2];
REAL yz = quat[1]*quat[2];
REAL wx = quat[3]*quat[0];
REAL wy = quat[3]*quat[1];
REAL wz = quat[3]*quat[2];
matrix[0*4+0] = 1 - 2 * ( yy + zz );
matrix[1*4+0] = 2 * ( xy - wz );
matrix[2*4+0] = 2 * ( xz + wy );
matrix[0*4+1] = 2 * ( xy + wz );
matrix[1*4+1] = 1 - 2 * ( xx + zz );
matrix[2*4+1] = 2 * ( yz - wx );
matrix[0*4+2] = 2 * ( xz - wy );
matrix[1*4+2] = 2 * ( yz + wx );
matrix[2*4+2] = 1 - 2 * ( xx + yy );
matrix[3*4+0] = matrix[3*4+1] = matrix[3*4+2] = (REAL) 0.0f;
matrix[0*4+3] = matrix[1*4+3] = matrix[2*4+3] = (REAL) 0.0f;
matrix[3*4+3] =(REAL) 1.0f;
}
void fm_quatRotate(const REAL *quat,const REAL *v,REAL *r) // rotate a vector
directly by a quaternion.
{
REAL left[4];
left[0] = quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
left[1] = quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
left[2] = quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-
quat[1]*left[2]);
r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-
quat[2]*left[0]);
r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-
quat[0]*left[1]);
void fm_getTranslation(const REAL *matrix,REAL *t)
{
t[0] = matrix[3*4+0];
t[1] = matrix[3*4+1];
t[2] = matrix[3*4+2];
}
void fm_matrixToQuat(const REAL *matrix,REAL *quat) // convert the 3x3 portion of a
4x4 matrix into a quaterion as x,y,z,w
{
REAL tr = matrix[0*4+0] + matrix[1*4+1] + matrix[2*4+2];
// check the diagonal
if (tr > 0.0f )
{
REAL s = (REAL) sqrt ( (double) (tr + 1.0f) );
quat[3] = s * 0.5f;
s = 0.5f / s;
quat[0] = (matrix[1*4+2] - matrix[2*4+1]) * s;
quat[1] = (matrix[2*4+0] - matrix[0*4+2]) * s;
quat[2] = (matrix[0*4+1] - matrix[1*4+0]) * s;
}
else
{
// diagonal is negative
int nxt[3] = {1, 2, 0};
REAL qa[4];
int i = 0;
if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
int j = nxt[i];
int k = nxt[j];
REAL s = sqrt ( ((matrix[i*4+i] - (matrix[j*4+j] + matrix[k*4+k])) + 1.0f) );
qa[i] = s * 0.5f;
if (s != 0.0f ) s = 0.5f / s;
qa[3] = (matrix[j*4+k] - matrix[k*4+j]) * s;
qa[j] = (matrix[i*4+j] + matrix[j*4+i]) * s;
qa[k] = (matrix[i*4+k] + matrix[k*4+i]) * s;
quat[0] = qa[0];
quat[1] = qa[1];
quat[2] = qa[2];
quat[3] = qa[3];
}
REAL fm_sphereVolume(REAL radius) // return's the volume of a sphere of this radius
(4/3 PI * R cubed )
{
return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
}
REAL fm_cylinderVolume(REAL radius,REAL h)
{
return FM_PI * radius * radius *h;
}
REAL fm_capsuleVolume(REAL radius,REAL h)
{
REAL volume = fm_sphereVolume(radius); // volume of the sphere portion.
REAL ch = h-radius*2; // this is the cylinder length
if ( ch > 0 )
{
volume+=fm_cylinderVolume(radius,ch);
}
return volume;
}
void fm_transform(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and
translate this point
{
if ( matrix )
{
REAL tx = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] *
v[2]) + matrix[3*4+0];
REAL ty = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] *
v[2]) + matrix[3*4+1];
REAL tz = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] *
v[2]) + matrix[3*4+2];
t[0] = tx;
t[1] = ty;
t[2] = tz;
}
else
{
t[0] = v[0];
t[1] = v[1];
t[2] = v[2];
}
}
void fm_rotate(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and
translate this point
{
if ( matrix )
{
REAL tx = (matrix[0*4+0] * v[0]) + (matrix[1*4+0] * v[1]) + (matrix[2*4+0] *
v[2]);
REAL ty = (matrix[0*4+1] * v[0]) + (matrix[1*4+1] * v[1]) + (matrix[2*4+1] *
v[2]);
REAL tz = (matrix[0*4+2] * v[0]) + (matrix[1*4+2] * v[1]) + (matrix[2*4+2] *
v[2]);
t[0] = tx;
t[1] = ty;
t[2] = tz;
}
else
{
t[0] = v[0];
t[1] = v[1];
t[2] = v[2];
}
}
REAL fm_distance(const REAL *p1,const REAL *p2)
{
REAL dx = p1[0] - p2[0];
REAL dy = p1[1] - p2[1];
REAL dz = p1[2] - p2[2];
return sqrt( dx*dx + dy*dy + dz *dz );
}
REAL fm_distanceSquared(const REAL *p1,const REAL *p2)
{
REAL dx = p1[0] - p2[0];
REAL dy = p1[1] - p2[1];
REAL dz = p1[2] - p2[2];
return dx*dx + dy*dy + dz *dz;
}
REAL fm_distanceSquaredXZ(const REAL *p1,const REAL *p2)
{
REAL dx = p1[0] - p2[0];
REAL dz = p1[2] - p2[2];
return dx*dx + dz *dz;
}
REAL fm_computePlane(const REAL *A,const REAL *B,const REAL *C,REAL *n) // returns
D
{
REAL vx = (B[0] - C[0]);
REAL vy = (B[1] - C[1]);
REAL vz = (B[2] - C[2]);
REAL wx = (A[0] - B[0]);
REAL wy = (A[1] - B[1]);
REAL wz = (A[2] - B[2]);
REAL vw_x = vy * wz - vz * wy;
REAL vw_y = vz * wx - vx * wz;
REAL vw_z = vx * wy - vy * wx;
REAL mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if ( mag < 0.000001f )
{
mag = 0;
}
else
{
mag = 1.0f/mag;
}
REAL x = vw_x * mag;
REAL y = vw_y * mag;
REAL z = vw_z * mag;
REAL D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
n[0] = x;
n[1] = y;
n[2] = z;
return D;
}
REAL fm_distToPlane(const REAL *plane,const REAL *p) // computes the distance of
this point from the plane.
{
return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
}
REAL fm_dot(const REAL *p1,const REAL *p2)
{
return p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2];
}
void fm_cross(REAL *cross,const REAL *a,const REAL *b)
{
cross[0] = a[1]*b[2] - a[2]*b[1];
cross[1] = a[2]*b[0] - a[0]*b[2];
cross[2] = a[0]*b[1] - a[1]*b[0];
}
void fm_computeNormalVector(REAL *n,const REAL *p1,const REAL *p2)
{
n[0] = p2[0] - p1[0];
n[1] = p2[1] - p1[1];
n[2] = p2[2] - p1[2];
fm_normalize(n);
}
bool fm_computeWindingOrder(const REAL *p1,const REAL *p2,const REAL *p3) //
returns true if the triangle is clockwise.
{
bool ret = false;
REAL v1[3];
REAL v2[3];
fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
REAL cross[3];
fm_cross(cross, v1, v2 );
REAL ref[3] = { 1, 0, 0 };
REAL d = fm_dot( cross, ref );
if ( d <= 0 )
ret = false;
else
ret = true;
return ret;
}
REAL fm_normalize(REAL *n) // normalize this vector
{
REAL dist = (REAL)sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
if ( dist > 0.0000001f )
{
REAL mag = 1.0f / dist;
n[0]*=mag;
n[1]*=mag;
n[2]*=mag;
}
else
{
n[0] = 1;
n[1] = 0;
n[2] = 0;
}
return dist;
}
void fm_matrixMultiply(const REAL *pA,const REAL *pB,REAL *pM)
{
#if 1
REAL a = pA[0*4+0] * pB[0*4+0] + pA[0*4+1] * pB[1*4+0] + pA[0*4+2] * pB[2*4+0] +
pA[0*4+3] * pB[3*4+0];
REAL b = pA[0*4+0] * pB[0*4+1] + pA[0*4+1] * pB[1*4+1] + pA[0*4+2] * pB[2*4+1] +
pA[0*4+3] * pB[3*4+1];
REAL c = pA[0*4+0] * pB[0*4+2] + pA[0*4+1] * pB[1*4+2] + pA[0*4+2] * pB[2*4+2] +
pA[0*4+3] * pB[3*4+2];
REAL d = pA[0*4+0] * pB[0*4+3] + pA[0*4+1] * pB[1*4+3] + pA[0*4+2] * pB[2*4+3] +
pA[0*4+3] * pB[3*4+3];
REAL e = pA[1*4+0] * pB[0*4+0] + pA[1*4+1] * pB[1*4+0] + pA[1*4+2] * pB[2*4+0] +
pA[1*4+3] * pB[3*4+0];
REAL f = pA[1*4+0] * pB[0*4+1] + pA[1*4+1] * pB[1*4+1] + pA[1*4+2] * pB[2*4+1] +
pA[1*4+3] * pB[3*4+1];
REAL g = pA[1*4+0] * pB[0*4+2] + pA[1*4+1] * pB[1*4+2] + pA[1*4+2] * pB[2*4+2] +
pA[1*4+3] * pB[3*4+2];
REAL h = pA[1*4+0] * pB[0*4+3] + pA[1*4+1] * pB[1*4+3] + pA[1*4+2] * pB[2*4+3] +
pA[1*4+3] * pB[3*4+3];
REAL i = pA[2*4+0] * pB[0*4+0] + pA[2*4+1] * pB[1*4+0] + pA[2*4+2] * pB[2*4+0] +
pA[2*4+3] * pB[3*4+0];
REAL j = pA[2*4+0] * pB[0*4+1] + pA[2*4+1] * pB[1*4+1] + pA[2*4+2] * pB[2*4+1] +
pA[2*4+3] * pB[3*4+1];
REAL k = pA[2*4+0] * pB[0*4+2] + pA[2*4+1] * pB[1*4+2] + pA[2*4+2] * pB[2*4+2] +
pA[2*4+3] * pB[3*4+2];
REAL l = pA[2*4+0] * pB[0*4+3] + pA[2*4+1] * pB[1*4+3] + pA[2*4+2] * pB[2*4+3] +
pA[2*4+3] * pB[3*4+3];
REAL m = pA[3*4+0] * pB[0*4+0] + pA[3*4+1] * pB[1*4+0] + pA[3*4+2] * pB[2*4+0] +
pA[3*4+3] * pB[3*4+0];
REAL n = pA[3*4+0] * pB[0*4+1] + pA[3*4+1] * pB[1*4+1] + pA[3*4+2] * pB[2*4+1] +
pA[3*4+3] * pB[3*4+1];
REAL o = pA[3*4+0] * pB[0*4+2] + pA[3*4+1] * pB[1*4+2] + pA[3*4+2] * pB[2*4+2] +
pA[3*4+3] * pB[3*4+2];
REAL p = pA[3*4+0] * pB[0*4+3] + pA[3*4+1] * pB[1*4+3] + pA[3*4+2] * pB[2*4+3] +
pA[3*4+3] * pB[3*4+3];
pM[0] = a;
pM[1] = b;
pM[2] = c;
pM[3] = d;
pM[4] = e;
pM[5] = f;
pM[6] = g;
pM[7] = h;
pM[8] = i;
pM[9] = j;
pM[10] = k;
pM[11] = l;
pM[12] = m;
pM[13] = n;
pM[14] = o;
pM[15] = p;
#else
memset(pM, 0, sizeof(REAL)*16);
for(int i=0; i<4; i++ )
for(int j=0; j<4; j++ )
for(int k=0; k<4; k++ )
pM[4*i+j] += pA[4*i+k] * pB[4*k+j];
#endif
}
void fm_eulerToQuatDX(REAL x,REAL y,REAL z,REAL *quat) // convert euler angles to
quaternion using the fucked up DirectX method
{
REAL matrix[16];
fm_eulerToMatrix(x,y,z,matrix);
fm_matrixToQuat(matrix,quat);
}
// implementation copied from:
http://blogs.msdn.com/mikepelton/archive/2004/10/29/249501.aspx
void fm_eulerToMatrixDX(REAL x,REAL y,REAL z,REAL *matrix) // convert euler angles
to quaternion using the fucked up DirectX method.
{
fm_identity(matrix);
matrix[0*4+0] = cos(z)*cos(y) + sin(z)*sin(x)*sin(y);
matrix[0*4+1] = sin(z)*cos(x);
matrix[0*4+2] = cos(z)*-sin(y) + sin(z)*sin(x)*cos(y);
matrix[1*4+0] = -sin(z)*cos(y)+cos(z)*sin(x)*sin(y);
matrix[1*4+1] = cos(z)*cos(x);
matrix[1*4+2] = sin(z)*sin(y) +cos(z)*sin(x)*cos(y);
matrix[2*4+0] = cos(x)*sin(y);
matrix[2*4+1] = -sin(x);
matrix[2*4+2] = cos(x)*cos(y);
}
void fm_scale(REAL x,REAL y,REAL z,REAL *fscale) // apply scale to the matrix.
{
fscale[0*4+0] = x;
fscale[1*4+1] = y;
fscale[2*4+2] = z;
}
void fm_composeTransform(const REAL *position,const REAL *quat,const REAL
*scale,REAL *matrix)
{
fm_identity(matrix);
fm_quatToMatrix(quat,matrix);
if ( scale && ( scale[0] != 1 || scale[1] != 1 || scale[2] != 1 ) )
{
REAL work[16];
memcpy(work,matrix,sizeof(REAL)*16);
REAL mscale[16];
fm_identity(mscale);
fm_scale(scale[0],scale[1],scale[2],mscale);
fm_matrixMultiply(work,mscale,matrix);
}
matrix[12] = position[0];
matrix[13] = position[1];
matrix[14] = position[2];
}
void fm_setTranslation(const REAL *translation,REAL *matrix)
{
matrix[12] = translation[0];
matrix[13] = translation[1];
matrix[14] = translation[2];
}
static REAL enorm0_3d ( REAL x0, REAL y0, REAL z0, REAL x1, REAL y1, REAL z1 )
/**********************************************************************/
/*
Purpose:
ENORM0_3D computes the Euclidean norm of (P1-P0) in 3D.
Modified:
18 April 1999
Author:
John Burkardt
Parameters:
Input, REAL X0, Y0, Z0, X1, Y1, Z1, the coordinates of the points
P0 and P1.
Output, REAL ENORM0_3D, the Euclidean norm of (P1-P0).
*/
{
REAL value;
value = sqrt (
( x1 - x0 ) * ( x1 - x0 ) +
( y1 - y0 ) * ( y1 - y0 ) +
( z1 - z0 ) * ( z1 - z0 ) );
return value;
}
static REAL triangle_area_3d ( REAL x1, REAL y1, REAL z1, REAL x2,REAL y2, REAL z2,
REAL x3, REAL y3, REAL z3 )
/**********************************************************************/
/*
Purpose:
TRIANGLE_AREA_3D computes the area of a triangle in 3D.
Modified:
22 April 1999
Author:
John Burkardt
Parameters:
Input, REAL X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (X,Y,Z)
coordinates of the corners of the triangle.
Output, REAL TRIANGLE_AREA_3D, the area of the triangle.
*/
{
REAL a;
REAL alpha;
REAL area;
REAL b;
REAL base;
REAL c;
REAL dot;
REAL height;
/*
Find the projection of (P3-P1) onto (P2-P1).
*/
dot =
( x2 - x1 ) * ( x3 - x1 ) +
( y2 - y1 ) * ( y3 - y1 ) +
( z2 - z1 ) * ( z3 - z1 );
base = enorm0_3d ( x1, y1, z1, x2, y2, z2 );
/*
The height of the triangle is the length of (P3-P1) after its
projection onto (P2-P1) has been subtracted.
*/
if ( base == 0.0 ) {
height = 0.0;
}
else {
alpha = dot / ( base * base );
a = x3 - x1 - alpha * ( x2 - x1 );
b = y3 - y1 - alpha * ( y2 - y1 );
c = z3 - z1 - alpha * ( z2 - z1 );
height = sqrt ( a * a + b * b + c * c );
}
area = 0.5f * base * height;
return area;
}
REAL fm_computeArea(const REAL *p1,const REAL *p2,const REAL *p3)
{
REAL ret = 0;
ret = triangle_area_3d(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2]);
return ret;
}
void fm_lerp(const REAL *p1,const REAL *p2,REAL *dest,REAL lerpValue)
{
dest[0] = ((p2[0] - p1[0])*lerpValue) + p1[0];
dest[1] = ((p2[1] - p1[1])*lerpValue) + p1[1];
dest[2] = ((p2[2] - p1[2])*lerpValue) + p1[2];
}
bool fm_pointTestXZ(const REAL *p,const REAL *i,const REAL *j)
{
bool ret = false;
if (((( i[2] <= p[2] ) && ( p[2] < j[2] )) || (( j[2] <= p[2] ) && ( p[2] <
i[2] ))) && ( p[0] < (j[0] - i[0]) * (p[2] - i[2]) / (j[2] - i[2]) + i[0]))
ret = true;
return ret;
};
bool fm_insideTriangleXZ(const REAL *p,const REAL *p1,const REAL *p2,const REAL
*p3)
{
bool ret = false;
int c = 0;
if ( fm_pointTestXZ(p,p1,p2) ) c = !c;
if ( fm_pointTestXZ(p,p2,p3) ) c = !c;
if ( fm_pointTestXZ(p,p3,p1) ) c = !c;
if ( c ) ret = true;
return ret;
}
bool fm_insideAABB(const REAL *pos,const REAL *bmin,const REAL *bmax)
{
bool ret = false;
if ( pos[0] >= bmin[0] && pos[0] <= bmax[0] &&
pos[1] >= bmin[1] && pos[1] <= bmax[1] &&
pos[2] >= bmin[2] && pos[2] <= bmax[2] )
ret = true;
return ret;
}
size_t fm_clipTestPoint(const REAL *bmin,const REAL *bmax,const REAL *pos)
{
size_t ret = 0;
if ( pos[0] < bmin[0] )
ret|=FMCS_XMIN;
else if ( pos[0] > bmax[0] )
ret|=FMCS_XMAX;
if ( pos[1] < bmin[1] )
ret|=FMCS_YMIN;
else if ( pos[1] > bmax[1] )
ret|=FMCS_YMAX;
if ( pos[2] < bmin[2] )
ret|=FMCS_ZMIN;
else if ( pos[2] > bmax[2] )
ret|=FMCS_ZMAX;
return ret;
}
size_t fm_clipTestPointXZ(const REAL *bmin,const REAL *bmax,const REAL *pos) //
only tests X and Z, not Y
{
size_t ret = 0;
if ( pos[0] < bmin[0] )
ret|=FMCS_XMIN;
else if ( pos[0] > bmax[0] )
ret|=FMCS_XMAX;
if ( pos[2] < bmin[2] )
ret|=FMCS_ZMIN;
else if ( pos[2] > bmax[2] )
ret|=FMCS_ZMAX;
return ret;
}
size_t fm_clipTestAABB(const REAL *bmin,const REAL *bmax,const REAL *p1,const REAL
*p2,const REAL *p3,size_t &andCode)
{
size_t orCode = 0;
andCode = FMCS_XMIN | FMCS_XMAX | FMCS_YMIN | FMCS_YMAX | FMCS_ZMIN | FMCS_ZMAX;
size_t c = fm_clipTestPoint(bmin,bmax,p1);
orCode|=c;
andCode&=c;
c = fm_clipTestPoint(bmin,bmax,p2);
orCode|=c;
andCode&=c;
c = fm_clipTestPoint(bmin,bmax,p3);
orCode|=c;
andCode&=c;
return orCode;
}
bool intersect(const REAL *si,const REAL *ei,const REAL *bmin,const REAL *bmax,REAL
*time)
{
REAL st,et,fst = 0,fet = 1;
for (int i = 0; i < 3; i++)
{
if (*si < *ei)
{
if (*si > *bmax || *ei < *bmin)
return false;
REAL di = *ei - *si;
st = (*si < *bmin)? (*bmin - *si) / di: 0;
et = (*ei > *bmax)? (*bmax - *si) / di: 1;
}
else
{
if (*ei > *bmax || *si < *bmin)
return false;
REAL di = *ei - *si;
st = (*si > *bmax)? (*bmax - *si) / di: 0;
et = (*ei < *bmin)? (*bmin - *si) / di: 1;
}
if (st > fst) fst = st;
if (et < fet) fet = et;
if (fet < fst)
return false;
bmin++; bmax++;
si++; ei++;
}
*time = fst;
return true;
}
bool fm_lineTestAABB(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL
*bmax,REAL &time)
{
bool sect = intersect(p1,p2,bmin,bmax,&time);
return sect;
}
bool fm_lineTestAABBXZ(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL
*bmax,REAL &time)
{
REAL _bmin[3];
REAL _bmax[3];
_bmin[0] = bmin[0];
_bmin[1] = -1e9;
_bmin[2] = bmin[2];
_bmax[0] = bmax[0];
_bmax[1] = 1e9;
_bmax[2] = bmax[2];
bool sect = intersect(p1,p2,_bmin,_bmax,&time);
return sect;
}
void fm_minmax(const REAL *p,REAL *bmin,REAL *bmax) // accmulate to a min-max
value
{
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
REAL fm_solveX(const REAL *plane,REAL y,REAL z) // solve for X given this plane
equation and the other two components.
{
REAL x = (y*plane[1]+z*plane[2]+plane[3]) / -plane[0];
return x;
}
REAL fm_solveY(const REAL *plane,REAL x,REAL z) // solve for Y given this plane
equation and the other two components.
{
REAL y = (x*plane[0]+z*plane[2]+plane[3]) / -plane[1];
return y;
}
REAL fm_solveZ(const REAL *plane,REAL x,REAL y) // solve for Y given this plane
equation and the other two components.
{
REAL z = (x*plane[0]+y*plane[1]+plane[3]) / -plane[2];
return z;
}
void fm_getAABBCenter(const REAL *bmin,const REAL *bmax,REAL *center)
{
center[0] = (bmax[0]-bmin[0])*0.5f+bmin[0];
center[1] = (bmax[1]-bmin[1])*0.5f+bmin[1];
center[2] = (bmax[2]-bmin[2])*0.5f+bmin[2];
}
FM_Axis fm_getDominantAxis(const REAL normal[3])
{
FM_Axis ret = FM_XAXIS;
REAL x = fabs(normal[0]);
REAL y = fabs(normal[1]);
REAL z = fabs(normal[2]);
if ( y > x && y > z )
ret = FM_YAXIS;
else if ( z > x && z > y )
ret = FM_ZAXIS;
return ret;
}
bool fm_lineSphereIntersect(const REAL *center,REAL radius,const REAL *p1,const
REAL *p2,REAL *intersect)
{
bool ret = false;
REAL dir[3];
dir[0] = p2[0]-p1[0];
dir[1] = p2[1]-p1[1];
dir[2] = p2[2]-p1[2];
REAL distance = sqrt( dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]);
if ( distance > 0 )
{
REAL recip = 1.0f / distance;
dir[0]*=recip;
dir[1]*=recip;
dir[2]*=recip;
ret = fm_raySphereIntersect(center,radius,p1,dir,distance,intersect);
}
else
{
dir[0] = center[0]-p1[0];
dir[1] = center[1]-p1[1];
dir[2] = center[2]-p1[2];
REAL d2 = dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2];
REAL r2 = radius*radius;
if ( d2 < r2 )
{
ret = true;
if ( intersect )
{
intersect[0] = p1[0];
intersect[1] = p1[1];
intersect[2] = p1[2];
}
}
}
return ret;
}
#define DOT(p1,p2) (p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2])
bool fm_raySphereIntersect(const REAL *center,REAL radius,const REAL *pos,const
REAL *dir,REAL distance,REAL *intersect)
{
bool ret = false;
REAL E0[3];
E0[0] = center[0] - pos[0];
E0[1] = center[1] - pos[1];
E0[2] = center[2] - pos[2];
REAL V[3];
V[0] = dir[0];
V[1] = dir[1];
V[2] = dir[2];
REAL dist2 = E0[0]*E0[0] + E0[1]*E0[1] + E0[2] * E0[2];
REAL radius2 = radius*radius; // radius squared..
// Bug Fix For Gem, if origin is *inside* the sphere, invert the
// direction vector so that we get a valid intersection location.
if ( dist2 < radius2 )
{
V[0]*=-1;
V[1]*=-1;
V[2]*=-1;
}
REAL v = DOT(E0,V);
REAL disc = radius2 - (dist2 - v*v);
if (disc > 0.0f)
{
if ( intersect )
{
REAL d = sqrt(disc);
REAL diff = v-d;
if ( diff < distance )
{
intersect[0] = pos[0]+V[0]*diff;
intersect[1] = pos[1]+V[1]*diff;
intersect[2] = pos[2]+V[2]*diff;
ret = true;
}
}
}
return ret;
}
void fm_catmullRom(REAL *out_vector,const REAL *p1,const REAL *p2,const REAL
*p3,const REAL *p4, const REAL s)
{
REAL s_squared = s * s;
REAL s_cubed = s_squared * s;
REAL coefficient_p1 = -s_cubed + 2*s_squared - s;
REAL coefficient_p2 = 3 * s_cubed - 5 * s_squared + 2;
REAL coefficient_p3 = -3 * s_cubed +4 * s_squared + s;
REAL coefficient_p4 = s_cubed - s_squared;
out_vector[0] = (coefficient_p1 * p1[0] + coefficient_p2 * p2[0] + coefficient_p3
* p3[0] + coefficient_p4 * p4[0])*0.5f;
out_vector[1] = (coefficient_p1 * p1[1] + coefficient_p2 * p2[1] + coefficient_p3
* p3[1] + coefficient_p4 * p4[1])*0.5f;
out_vector[2] = (coefficient_p1 * p1[2] + coefficient_p2 * p2[2] + coefficient_p3
* p3[2] + coefficient_p4 * p4[2])*0.5f;
}
bool fm_intersectAABB(const REAL *bmin1,const REAL *bmax1,const REAL *bmin2,const
REAL *bmax2)
{
if ((bmin1[0] > bmax2[0]) || (bmin2[0] > bmax1[0])) return false;
if ((bmin1[1] > bmax2[1]) || (bmin2[1] > bmax1[1])) return false;
if ((bmin1[2] > bmax2[2]) || (bmin2[2] > bmax1[2])) return false;
return true;
bool fm_insideAABB(const REAL *obmin,const REAL *obmax,const REAL *tbmin,const
REAL *tbmax) // test if bounding box tbmin/tmbax is fully inside obmin/obmax
{
bool ret = false;
if ( tbmax[0] <= obmax[0] &&
tbmax[1] <= obmax[1] &&
tbmax[2] <= obmax[2] &&
tbmin[0] >= obmin[0] &&
tbmin[1] >= obmin[1] &&
tbmin[2] >= obmin[2] ) ret = true;
return ret;
}
// Reference, from Stan Melax in Game Gems I
// Quaternion q;
// vector3 c = CrossProduct(v0,v1);
// REAL d = DotProduct(v0,v1);
// REAL s = (REAL)sqrt((1+d)*2);
// q.x = c.x / s;
// q.y = c.y / s;
// q.z = c.z / s;
// q.w = s /2.0f;
// return q;
void fm_rotationArc(const REAL *v0,const REAL *v1,REAL *quat)
{
REAL cross[3];
fm_cross(cross,v0,v1);
REAL d = fm_dot(v0,v1);
REAL s = sqrt((1+d)*2);
REAL recip = 1.0f / s;
quat[0] = cross[0] * recip;
quat[1] = cross[1] * recip;
quat[2] = cross[2] * recip;
quat[3] = s * 0.5f;
REAL fm_distancePointLineSegment(const REAL *Point,const REAL *LineStart,const REAL
*LineEnd,REAL *intersection,LineSegmentType &type)
{
REAL ret;
REAL LineMag = fm_distance( LineEnd, LineStart );
if ( LineMag > 0.000001f )
{
REAL U = ( ( ( Point[0] - LineStart[0] ) * ( LineEnd[0] - LineStart[0] ) ) +
( ( Point[1] - LineStart[1] ) * ( LineEnd[1] - LineStart[1] ) ) + ( ( Point[2] -
LineStart[2] ) * ( LineEnd[2] - LineStart[2] ) ) ) / ( LineMag * LineMag );
if( U < 0.0f || U > 1.0f )
{
REAL d1 = fm_distanceSquared(Point,LineStart);
REAL d2 = fm_distanceSquared(Point,LineEnd);
if ( d1 <= d2 )
{
ret = sqrt(d1);
intersection[0] = LineStart[0];
intersection[1] = LineStart[1];
intersection[2] = LineStart[2];
type = LS_START;
}
else
{
ret = sqrt(d2);
intersection[0] = LineEnd[0];
intersection[1] = LineEnd[1];
intersection[2] = LineEnd[2];
type = LS_END;
}
}
else
{
intersection[0]= LineStart[0] + U * ( LineEnd[0] - LineStart[0] );
intersection[1]= LineStart[1] + U * ( LineEnd[1] - LineStart[1] );
intersection[2]= LineStart[2] + U * ( LineEnd[2] - LineStart[2] );
ret = fm_distance(Point,intersection);
if ( U < 0.01f ) // if less than 1/100th the total distance, treat is as the
'start'
{
type = LS_START;
}
else if ( U > 0.99f )
{
type = LS_END;
}
else
{
type = LS_MIDDLE;
}
}
}
else
{
ret = LineMag;
intersection[0] = LineEnd[0];
intersection[1] = LineEnd[1];
intersection[2] = LineEnd[2];
type = LS_END;
}
return ret;
}
#ifndef BEST_FIT_PLANE_H
#define BEST_FIT_PLANE_H
template <class Type> class Eigen
{
public:
void DecrSortEigenStuff(void)
{
Tridiagonal(); //diagonalize the matrix.
QLAlgorithm(); //
DecreasingSort();
GuaranteeRotation();
}
void Tridiagonal(void)
{
Type fM00 = mElement[0][0];
Type fM01 = mElement[0][1];
Type fM02 = mElement[0][2];
Type fM11 = mElement[1][1];
Type fM12 = mElement[1][2];
Type fM22 = mElement[2][2];
m_afDiag[0] = fM00;
m_afSubd[2] = 0;
if (fM02 != (Type)0.0)
{
Type fLength = sqrt(fM01*fM01+fM02*fM02);
Type fInvLength = ((Type)1.0)/fLength;
fM01 *= fInvLength;
fM02 *= fInvLength;
Type fQ = ((Type)2.0)*fM01*fM12+fM02*(fM22-fM11);
m_afDiag[1] = fM11+fM02*fQ;
m_afDiag[2] = fM22-fM02*fQ;
m_afSubd[0] = fLength;
m_afSubd[1] = fM12-fM01*fQ;
mElement[0][0] = (Type)1.0;
mElement[0][1] = (Type)0.0;
mElement[0][2] = (Type)0.0;
mElement[1][0] = (Type)0.0;
mElement[1][1] = fM01;
mElement[1][2] = fM02;
mElement[2][0] = (Type)0.0;
mElement[2][1] = fM02;
mElement[2][2] = -fM01;
m_bIsRotation = false;
}
else
{
m_afDiag[1] = fM11;
m_afDiag[2] = fM22;
m_afSubd[0] = fM01;
m_afSubd[1] = fM12;
mElement[0][0] = (Type)1.0;
mElement[0][1] = (Type)0.0;
mElement[0][2] = (Type)0.0;
mElement[1][0] = (Type)0.0;
mElement[1][1] = (Type)1.0;
mElement[1][2] = (Type)0.0;
mElement[2][0] = (Type)0.0;
mElement[2][1] = (Type)0.0;
mElement[2][2] = (Type)1.0;
m_bIsRotation = true;
}
}
bool QLAlgorithm(void)
{
const int iMaxIter = 32;
for (int i0 = 0; i0 <3; i0++)
{
int i1;
for (i1 = 0; i1 < iMaxIter; i1++)
{
int i2;
for (i2 = i0; i2 <= (3-2); i2++)
{
Type fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
break;
}
if (i2 == i0)
{
break;
}
Type fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((Type)2.0) * m_afSubd[i0]);
Type fR = sqrt(fG*fG+(Type)1.0);
if (fG < (Type)0.0)
{
fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
}
else
{
fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
}
Type fSin = (Type)1.0, fCos = (Type)1.0, fP = (Type)0.0;
for (int i3 = i2-1; i3 >= i0; i3--)
{
Type fF = fSin*m_afSubd[i3];
Type fB = fCos*m_afSubd[i3];
if (fabs(fF) >= fabs(fG))
{
fCos = fG/fF;
fR = sqrt(fCos*fCos+(Type)1.0);
m_afSubd[i3+1] = fF*fR;
fSin = ((Type)1.0)/fR;
fCos *= fSin;
}
else
{
fSin = fF/fG;
fR = sqrt(fSin*fSin+(Type)1.0);
m_afSubd[i3+1] = fG*fR;
fCos = ((Type)1.0)/fR;
fSin *= fCos;
}
fG = m_afDiag[i3+1]-fP;
fR = (m_afDiag[i3]-fG)*fSin+((Type)2.0)*fB*fCos;
fP = fSin*fR;
m_afDiag[i3+1] = fG+fP;
fG = fCos*fR-fB;
for (int i4 = 0; i4 < 3; i4++)
{
fF = mElement[i4][i3+1];
mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
}
}
m_afDiag[i0] -= fP;
m_afSubd[i0] = fG;
m_afSubd[i2] = (Type)0.0;
}
if (i1 == iMaxIter)
{
return false;
}
}
return true;
}
void DecreasingSort(void)
{
//sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
for (int i0 = 0, i1; i0 <= 3-2; i0++)
{
// locate maximum eigenvalue
i1 = i0;
Type fMax = m_afDiag[i1];
int i2;
for (i2 = i0+1; i2 < 3; i2++)
{
if (m_afDiag[i2] > fMax)
{
i1 = i2;
fMax = m_afDiag[i1];
}
}
if (i1 != i0)
{
// swap eigenvalues
m_afDiag[i1] = m_afDiag[i0];
m_afDiag[i0] = fMax;
// swap eigenvectors
for (i2 = 0; i2 < 3; i2++)
{
Type fTmp = mElement[i2][i0];
mElement[i2][i0] = mElement[i2][i1];
mElement[i2][i1] = fTmp;
m_bIsRotation = !m_bIsRotation;
}
}
}
}
void GuaranteeRotation(void)
{
if (!m_bIsRotation)
{
// change sign on the first column
for (int iRow = 0; iRow <3; iRow++)
{
mElement[iRow][0] = -mElement[iRow][0];
}
}
}
Type mElement[3][3];
Type m_afDiag[3];
Type m_afSubd[3];
bool m_bIsRotation;
};
#endif
bool fm_computeBestFitPlane(size_t vcount,
const REAL *points,
size_t vstride,
const REAL *weights,
size_t wstride,
REAL *plane)
{
bool ret = false;
REAL kOrigin[3] = { 0, 0, 0 };
REAL wtotal = 0;
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *) source;
REAL w = 1;
if ( wsource )
{
const REAL *ws = (const REAL *) wsource;
w = *ws; //
wsource+=wstride;
}
kOrigin[0]+=p[0]*w;
kOrigin[1]+=p[1]*w;
kOrigin[2]+=p[2]*w;
wtotal+=w;
source+=vstride;
}
}
REAL recip = 1.0f / wtotal; // reciprocol of total weighting
kOrigin[0]*=recip;
kOrigin[1]*=recip;
kOrigin[2]*=recip;
REAL fSumXX=0;
REAL fSumXY=0;
REAL fSumXZ=0;
REAL fSumYY=0;
REAL fSumYZ=0;
REAL fSumZZ=0;
{
const char *source = (const char *) points;
const char *wsource = (const char *) weights;
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *) source;
REAL w = 1;
if ( wsource )
{
const REAL *ws = (const REAL *) wsource;
w = *ws; //
wsource+=wstride;
}
REAL kDiff[3];
kDiff[0] = w*(p[0] - kOrigin[0]); // apply vertex weighting!
kDiff[1] = w*(p[1] - kOrigin[1]);
kDiff[2] = w*(p[2] - kOrigin[2]);
fSumXX+= kDiff[0] * kDiff[0]; // sume of the squares of the differences.
fSumXY+= kDiff[0] * kDiff[1]; // sume of the squares of the differences.
fSumXZ+= kDiff[0] * kDiff[2]; // sume of the squares of the differences.
fSumYY+= kDiff[1] * kDiff[1];
fSumYZ+= kDiff[1] * kDiff[2];
fSumZZ+= kDiff[2] * kDiff[2];
source+=vstride;
}
}
fSumXX *= recip;
fSumXY *= recip;
fSumXZ *= recip;
fSumYY *= recip;
fSumYZ *= recip;
fSumZZ *= recip;
// setup the eigensolver
Eigen<REAL> kES;
kES.mElement[0][0] = fSumXX;
kES.mElement[0][1] = fSumXY;
kES.mElement[0][2] = fSumXZ;
kES.mElement[1][0] = fSumXY;
kES.mElement[1][1] = fSumYY;
kES.mElement[1][2] = fSumYZ;
kES.mElement[2][0] = fSumXZ;
kES.mElement[2][1] = fSumYZ;
kES.mElement[2][2] = fSumZZ;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff();
REAL kNormal[3];
kNormal[0] = kES.mElement[0][2];
kNormal[1] = kES.mElement[1][2];
kNormal[2] = kES.mElement[2][2];
// the minimum energy
plane[0] = kNormal[0];
plane[1] = kNormal[1];
plane[2] = kNormal[2];
plane[3] = 0 - fm_dot(kNormal,kOrigin);
ret = true;
return ret;
}
bool fm_colinear(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)
{
bool ret = false;
REAL dir1[3];
REAL dir2[3];
dir1[0] = p2[0] - p1[0];
dir1[1] = p2[1] - p1[1];
dir1[2] = p2[2] - p1[2];
dir2[0] = p3[0] - p2[0];
dir2[1] = p3[1] - p2[1];
dir2[2] = p3[2] - p2[2];
fm_normalize(dir1);
fm_normalize(dir2);
REAL dot = fm_dot(dir1,dir2);
if ( dot >= epsilon )
{
ret = true;
}
return ret;
}
void fm_initMinMax(const REAL *p,REAL *bmin,REAL *bmax)
{
bmax[0] = bmin[0] = p[0];
bmax[1] = bmin[1] = p[1];
bmax[2] = bmin[2] = p[2];
}
IntersectResult fm_intersectLineSegments2d(const REAL *a1,const REAL *a2,const REAL
*b1,const REAL *b2,REAL *intersection)
{
IntersectResult ret;
REAL denom = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] -
a1[1]));
REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] -
b1[0]));
REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] -
b1[0]));
if (denom == 0 )
{
if(nume_a == 0 && nume_b == 0)
{
ret = IR_COINCIDENT;
}
else
{
ret = IR_PARALLEL;
}
}
else
{
REAL recip = 1 / denom;
REAL ua = nume_a * recip;
REAL ub = nume_b * recip;
if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )
{
// Get the intersection point.
intersection[0] = a1[0] + ua*(a2[0] - a1[0]);
intersection[1] = a1[1] + ua*(a2[1] - a1[1]);
ret = IR_DO_INTERSECT;
}
else
{
ret = IR_DONT_INTERSECT;
}
}
return ret;
}
IntersectResult fm_intersectLineSegments2dTime(const REAL *a1,const REAL *a2,const
REAL *b1,const REAL *b2,REAL &t1,REAL &t2)
{
IntersectResult ret;
REAL denom = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] -
a1[1]));
REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] -
b1[0]));
REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] -
b1[0]));
if (denom == 0 )
{
if(nume_a == 0 && nume_b == 0)
{
ret = IR_COINCIDENT;
}
else
{
ret = IR_PARALLEL;
}
}
else
{
REAL recip = 1 / denom;
REAL ua = nume_a * recip;
REAL ub = nume_b * recip;
if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )
{
t1 = ua;
t2 = ub;
ret = IR_DO_INTERSECT;
}
else
{
ret = IR_DONT_INTERSECT;
}
}
return ret;
}
//**** Plane Triangle Intersection
// assumes that the points are on opposite sides of the plane!
void fm_intersectPointPlane(const REAL *p1,const REAL *p2,REAL *split,const REAL
*plane)
{
REAL dp1 = fm_distToPlane(plane,p1);
REAL dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
REAL dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
REAL dot2 = dp1 - plane[3];
REAL t = -(plane[3] + dot2 ) / dot1;
split[0] = (dir[0]*t)+p1[0];
split[1] = (dir[1]*t)+p1[1];
split[2] = (dir[2]*t)+p1[2];
PlaneTriResult fm_getSidePlane(const REAL *p,const REAL *plane,REAL epsilon)
{
PlaneTriResult ret = PTR_ON_PLANE;
REAL d = fm_distToPlane(plane,p);
if ( d < -epsilon || d > epsilon )
{
if ( d > 0 )
ret = PTR_FRONT; // it is 'in front' within the provided epsilon value.
else
ret = PTR_BACK;
}
return ret;
}
#ifndef PLANE_TRIANGLE_INTERSECTION_H
#define PLANE_TRIANGLE_INTERSECTION_H
#define MAXPTS 256
template <class Type> class point
{
public:
void set(const Type *p)
{
x = p[0];
y = p[1];
z = p[2];
}
Type x;
Type y;
Type z;
};
template <class Type> class plane
{
public:
plane(const Type *p)
{
normal.x = p[0];
normal.y = p[1];
normal.z = p[2];
D = p[3];
}
Type Classify_Point(const point<Type> &p)
{
return p.x*normal.x + p.y*normal.y + p.z*normal.z + D;
}
point<Type> normal;
Type D;
};
template <class Type> class polygon
{
public:
polygon(void)
{
mVcount = 0;
}
polygon(const Type *p1,const Type *p2,const Type *p3)
{
mVcount = 3;
mVertices[0].set(p1);
mVertices[1].set(p2);
mVertices[2].set(p3);
}
int NumVertices(void) const { return mVcount; };
const point<Type>& Vertex(int index)
{
if ( index < 0 ) index+=mVcount;
return mVertices[index];
};
void set(const point<Type> *pts,int count)
{
for (int i=0; i<count; i++)
{
mVertices[i] = pts[i];
}
mVcount = count;
}
void Split_Polygon(polygon<Type> *poly,plane<Type> *part, polygon<Type> &front,
polygon<Type> &back)
{
int count = poly->NumVertices ();
int out_c = 0, in_c = 0;
point<Type> ptA, ptB,outpts[MAXPTS],inpts[MAXPTS];
Type sideA, sideB;
ptA = poly->Vertex (count - 1);
sideA = part->Classify_Point (ptA);
for (int i = -1; ++i < count;)
{
ptB = poly->Vertex(i);
sideB = part->Classify_Point(ptB);
if (sideB > 0)
{
if (sideA < 0)
{
point<Type> v;
fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );
outpts[out_c++] = inpts[in_c++] = v;
}
outpts[out_c++] = ptB;
}
else if (sideB < 0)
{
if (sideA > 0)
{
point<Type> v;
fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );
outpts[out_c++] = inpts[in_c++] = v;
}
inpts[in_c++] = ptB;
}
else
outpts[out_c++] = inpts[in_c++] = ptB;
ptA = ptB;
sideA = sideB;
}
front.set(&outpts[0], out_c);
back.set(&inpts[0], in_c);
}
int mVcount;
point<Type> mVertices[MAXPTS];
};
#endif
static inline void add(const REAL *p,REAL *dest,size_t tstride,size_t &pcount)
{
char *d = (char *) dest;
d = d + pcount*tstride;
dest = (REAL *) d;
dest[0] = p[0];
dest[1] = p[1];
dest[2] = p[2];
pcount++;
assert( pcount <= 4 );
}
PlaneTriResult fm_planeTriIntersection(const REAL *_plane, // the plane equation
in Ax+By+Cz+D format
const REAL *triangle, // the source triangle.
size_t tstride, // stride in bytes of the
input and output *vertices*
REAL epsilon, // the co-planar epsilon
value.
REAL *front, // the triangle in front
of the
size_t &fcount, // number of vertices in the
'front' triangle
REAL *back, // the triangle in back
of the plane
size_t &bcount) // the number of vertices in
the 'back' triangle.
{
fcount = 0;
bcount = 0;
const char *tsource = (const char *) triangle;
// get the three vertices of the triangle.
const REAL *p1 = (const REAL *) (tsource);
const REAL *p2 = (const REAL *) (tsource+tstride);
const REAL *p3 = (const REAL *) (tsource+tstride*2);
PlaneTriResult r1 = fm_getSidePlane(p1,_plane,epsilon); // compute the side of
the plane each vertex is on
PlaneTriResult r2 = fm_getSidePlane(p2,_plane,epsilon);
PlaneTriResult r3 = fm_getSidePlane(p3,_plane,epsilon);
// If any of the points lay right *on* the plane....
if ( r1 == PTR_ON_PLANE || r2 == PTR_ON_PLANE || r3 == PTR_ON_PLANE )
{
// If the triangle is completely co-planar, then just treat it as 'front' and
return!
if ( r1 == PTR_ON_PLANE && r2 == PTR_ON_PLANE && r3 == PTR_ON_PLANE )
{
add(p1,front,tstride,fcount);
add(p2,front,tstride,fcount);
add(p3,front,tstride,fcount);
return PTR_FRONT;
}
// Decide to place the co-planar points on the same side as the co-planar
point.
PlaneTriResult r= PTR_ON_PLANE;
if ( r1 != PTR_ON_PLANE )
r = r1;
else if ( r2 != PTR_ON_PLANE )
r = r2;
else if ( r3 != PTR_ON_PLANE )
r = r3;
if ( r1 == PTR_ON_PLANE ) r1 = r;
if ( r2 == PTR_ON_PLANE ) r2 = r;
if ( r3 == PTR_ON_PLANE ) r3 = r;
if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the
plane.
{
if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to
the 'front' output triangle.
{
add(p1,front,tstride,fcount);
add(p2,front,tstride,fcount);
add(p3,front,tstride,fcount);
}
else
{
add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the
'back' output triangle.
add(p2,back,tstride,bcount);
add(p3,back,tstride,bcount);
}
return r1; // if all three points are on the same side of the plane return
result
}
polygon<REAL> pi(p1,p2,p3);
polygon<REAL> pfront,pback;
plane<REAL> part(_plane);
pi.Split_Polygon(&pi,&part,pfront,pback);
for (int i=0; i<pfront.mVcount; i++)
{
add( &pfront.mVertices[i].x, front, tstride, fcount );
}
for (int i=0; i<pback.mVcount; i++)
{
add( &pback.mVertices[i].x, back, tstride, bcount );
}
PlaneTriResult ret = PTR_SPLIT;
if ( fcount < 3 ) fcount = 0;
if ( bcount < 3 ) bcount = 0;
if ( fcount == 0 && bcount )
ret = PTR_BACK;
if ( bcount == 0 && fcount )
ret = PTR_FRONT;
return ret;
}
// computes the OBB for this set of points relative to this transform matrix.
void computeOBB(size_t vcount,const REAL *points,size_t pstride,REAL *sides,REAL
*matrix)
{
const char *src = (const char *) points;
REAL bmin[3] = { 1e9, 1e9, 1e9 };
REAL bmax[3] = { -1e9, -1e9, -1e9 };
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *) src;
REAL t[3];
fm_inverseRT(matrix, p, t ); // inverse rotate translate
if ( t[0] < bmin[0] ) bmin[0] = t[0];
if ( t[1] < bmin[1] ) bmin[1] = t[1];
if ( t[2] < bmin[2] ) bmin[2] = t[2];
if ( t[0] > bmax[0] ) bmax[0] = t[0];
if ( t[1] > bmax[1] ) bmax[1] = t[1];
if ( t[2] > bmax[2] ) bmax[2] = t[2];
src+=pstride;
}
REAL center[3];
sides[0] = bmax[0]-bmin[0];
sides[1] = bmax[1]-bmin[1];
sides[2] = bmax[2]-bmin[2];
center[0] = sides[0]*0.5f+bmin[0];
center[1] = sides[1]*0.5f+bmin[1];
center[2] = sides[2]*0.5f+bmin[2];
REAL ocenter[3];
fm_rotate(matrix,center,ocenter);
matrix[12]+=ocenter[0];
matrix[13]+=ocenter[1];
matrix[14]+=ocenter[2];
void fm_computeBestFitOBB(size_t vcount,const REAL *points,size_t pstride,REAL
*sides,REAL *matrix,FitStrategy strategy)
{
fm_identity(matrix);
REAL bmin[3];
REAL bmax[3];
fm_computeBestFitAABB(vcount,points,pstride,bmin,bmax);
REAL avolume = (bmax[0]-bmin[0])*(bmax[1]-bmin[1])*(bmax[2]-bmin[2]);
REAL plane[4];
fm_computeBestFitPlane(vcount,points,pstride,0,0,plane);
fm_planeToMatrix(plane,matrix);
computeOBB( vcount, points, pstride, sides, matrix );
REAL refmatrix[16];
memcpy(refmatrix,matrix,16*sizeof(REAL));
REAL volume = sides[0]*sides[1]*sides[2];
float stepSize=5;
switch ( strategy )
{
case FS_FAST_FIT:
stepSize = 13; // 15 degree increments
break;
case FS_MEDIUM_FIT:
stepSize = 7; // 10 degree increments
break;
case FS_SLOW_FIT:
stepSize = 3; // 5 degree increments
break;
}
for (REAL a=0; a<180; a+=stepSize)
{
REAL quat[4];
fm_eulerToQuat(0,a*FM_DEG_TO_RAD,0,quat);
REAL temp[16];
REAL pmatrix[16];
fm_quatToMatrix(quat,temp);
fm_matrixMultiply(temp,refmatrix,pmatrix);
REAL psides[3];
computeOBB( vcount, points, pstride, psides, pmatrix );
REAL v = psides[0]*psides[1]*psides[2];
if ( v < volume )
{
volume = v;
memcpy(matrix,pmatrix,sizeof(REAL)*16);
sides[0] = psides[0];
sides[1] = psides[1];
sides[2] = psides[2];
}
}
if ( avolume < volume )
{
fm_identity(matrix);
matrix[12] = (bmin[0]+bmax[0])*0.5f;
matrix[13] = (bmin[1]+bmax[1])*0.5f;
matrix[14] = (bmin[2]+bmax[2])*0.5f;
sides[0] = bmax[0]-bmin[0];
sides[1] = bmax[1]-bmin[1];
sides[2] = bmax[2]-bmin[2];
}
}
void fm_computeBestFitOBB(size_t vcount,const REAL *points,size_t pstride,REAL
*sides,REAL *pos,REAL *quat,FitStrategy strategy)
{
REAL matrix[16];
fm_computeBestFitOBB(vcount,points,pstride,sides,matrix,strategy);
fm_getTranslation(matrix,pos);
fm_matrixToQuat(matrix,quat);
}
void fm_computeBestFitABB(size_t vcount,const REAL *points,size_t pstride,REAL
*sides,REAL *pos)
{
REAL bmin[3];
REAL bmax[3];
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
const char *cp = (const char *) points;
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *) cp;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
cp+=pstride;
}
sides[0] = bmax[0] - bmin[0];
sides[1] = bmax[1] - bmin[1];
sides[2] = bmax[2] - bmin[2];
pos[0] = bmin[0]+sides[0]*0.5f;
pos[1] = bmin[1]+sides[1]*0.5f;
pos[2] = bmin[2]+sides[2]*0.5f;
void fm_planeToMatrix(const REAL *plane,REAL *matrix) // convert a plane equation
to a 4x4 rotation matrix
{
REAL ref[3] = { 0, 1, 0 };
REAL quat[4];
fm_rotationArc(ref,plane,quat);
fm_quatToMatrix(quat,matrix);
REAL origin[3] = { 0, -plane[3], 0 };
REAL center[3];
fm_transform(matrix,origin,center);
fm_setTranslation(center,matrix);
}
void fm_planeToQuat(const REAL *plane,REAL *quat,REAL *pos) // convert a plane
equation to a quaternion and translation
{
REAL ref[3] = { 0, 1, 0 };
REAL matrix[16];
fm_rotationArc(ref,plane,quat);
fm_quatToMatrix(quat,matrix);
REAL origin[3] = { 0, plane[3], 0 };
fm_transform(matrix,origin,pos);
}
void fm_eulerMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in
radians) to a dest 4x4 matrix (translation set to zero)
{
REAL quat[4];
fm_eulerToQuat(ax,ay,az,quat);
fm_quatToMatrix(quat,matrix);
}
//**********************************************************
//**********************************************************
//**** Vertex Welding
//**********************************************************
//**********************************************************
#ifndef VERTEX_INDEX_H
#define VERTEX_INDEX_H
#include <vector>
#include <vector>
namespace VERTEX_INDEX
{
class KdTreeNode;
typedef USER_STL::vector< KdTreeNode * > KdTreeNodeVector;
enum Axes
{
X_AXIS = 0,
Y_AXIS = 1,
Z_AXIS = 2
};
class KdTreeFindNode
{
public:
KdTreeFindNode(void)
{
mNode = 0;
mDistance = 0;
}
KdTreeNode *mNode;
double mDistance;
};
class KdTreeInterface
{
public:
virtual const double * getPositionDouble(size_t index) const = 0;
virtual const float * getPositionFloat(size_t index) const = 0;
};
class KdTreeNode
{
public:
KdTreeNode(void)
{
mIndex = 0;
mLeft = 0;
mRight = 0;
}
KdTreeNode(size_t index)
{
mIndex = index;
mLeft = 0;
mRight = 0;
};
~KdTreeNode(void)
{
}
void addDouble(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)
{
const double *nodePosition = iface->getPositionDouble( node->mIndex );
const double *position = iface->getPositionDouble( mIndex );
switch ( dim )
{
case X_AXIS:
if ( nodePosition[0] <= position[0] )
{
if ( mLeft )
mLeft->addDouble(node,Y_AXIS,iface);
else
mLeft = node;
}
else
{
if ( mRight )
mRight->addDouble(node,Y_AXIS,iface);
else
mRight = node;
}
break;
case Y_AXIS:
if ( nodePosition[1] <= position[1] )
{
if ( mLeft )
mLeft->addDouble(node,Z_AXIS,iface);
else
mLeft = node;
}
else
{
if ( mRight )
mRight->addDouble(node,Z_AXIS,iface);
else
mRight = node;
}
break;
case Z_AXIS:
if ( nodePosition[2] <= position[2] )
{
if ( mLeft )
mLeft->addDouble(node,X_AXIS,iface);
else
mLeft = node;
}
else
{
if ( mRight )
mRight->addDouble(node,X_AXIS,iface);
else
mRight = node;
}
break;
}
void addFloat(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)
{
const float *nodePosition = iface->getPositionFloat( node->mIndex );
const float *position = iface->getPositionFloat( mIndex );
switch ( dim )
{
case X_AXIS:
if ( nodePosition[0] <= position[0] )
{
if ( mLeft )
mLeft->addFloat(node,Y_AXIS,iface);
else
mLeft = node;
}
else
{
if ( mRight )
mRight->addFloat(node,Y_AXIS,iface);
else
mRight = node;
}
break;
case Y_AXIS:
if ( nodePosition[1] <= position[1] )
{
if ( mLeft )
mLeft->addFloat(node,Z_AXIS,iface);
else
mLeft = node;
}
else
{
if ( mRight )
mRight->addFloat(node,Z_AXIS,iface);
else
mRight = node;
}
break;
case Z_AXIS:
if ( nodePosition[2] <= position[2] )
{
if ( mLeft )
mLeft->addFloat(node,X_AXIS,iface);
else
mLeft = node;
}
else
{
if ( mRight )
mRight->addFloat(node,X_AXIS,iface);
else
mRight = node;
}
break;
}
size_t getIndex(void) const { return mIndex; };
void search(Axes axis,const double *pos,double radius,size_t &count,size_t
maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)
{
const double *position = iface->getPositionDouble(mIndex);
double dx = pos[0] - position[0];
double dy = pos[1] - position[1];
double dz = pos[2] - position[2];
KdTreeNode *search1 = 0;
KdTreeNode *search2 = 0;
switch ( axis )
{
case X_AXIS:
if ( dx <= 0 ) // JWR if we are to the left
{
search1 = mLeft; // JWR then search to the left
if ( -dx < radius ) // JWR if distance to the right is less than our
search radius, continue on the right as well.
search2 = mRight;
}
else
{
search1 = mRight; // JWR ok, we go down the left tree
if ( dx < radius ) // JWR if the distance from the right is less than our
search radius
search2 = mLeft;
}
axis = Y_AXIS;
break;
case Y_AXIS:
if ( dy <= 0 )
{
search1 = mLeft;
if ( -dy < radius )
search2 = mRight;
}
else
{
search1 = mRight;
if ( dy < radius )
search2 = mLeft;
}
axis = Z_AXIS;
break;
case Z_AXIS:
if ( dz <= 0 )
{
search1 = mLeft;
if ( -dz < radius )
search2 = mRight;
}
else
{
search1 = mRight;
if ( dz < radius )
search2 = mLeft;
}
axis = X_AXIS;
break;
}
double r2 = radius*radius;
double m = dx*dx+dy*dy+dz*dz;
if ( m < r2 )
{
switch ( count )
{
case 0:
found[count].mNode = this;
found[count].mDistance = m;
break;
case 1:
if ( m < found[0].mDistance )
{
if ( maxObjects == 1 )
{
found[0].mNode = this;
found[0].mDistance = m;
}
else
{
found[1] = found[0];
found[0].mNode = this;
found[0].mDistance = m;
}
}
else if ( maxObjects > 1)
{
found[1].mNode = this;
found[1].mDistance = m;
}
break;
default:
{
bool inserted = false;
for (size_t i=0; i<count; i++)
{
if ( m < found[i].mDistance ) // if this one is closer than a pre-
existing one...
{
// insertion sort...
size_t scan = count;
if ( scan >= maxObjects ) scan=maxObjects-1;
for (size_t j=scan; j>i; j--)
{
found[j] = found[j-1];
}
found[i].mNode = this;
found[i].mDistance = m;
inserted = true;
break;
}
}
if ( !inserted && count < maxObjects )
{
found[count].mNode = this;
found[count].mDistance = m;
}
}
break;
}
count++;
if ( count > maxObjects )
{
count = maxObjects;
}
}
if ( search1 )
search1->search( axis, pos,radius, count, maxObjects, found, iface);
if ( search2 )
search2->search( axis, pos,radius, count, maxObjects, found, iface);
void search(Axes axis,const float *pos,float radius,size_t &count,size_t
maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)
{
const float *position = iface->getPositionFloat(mIndex);
float dx = pos[0] - position[0];
float dy = pos[1] - position[1];
float dz = pos[2] - position[2];
KdTreeNode *search1 = 0;
KdTreeNode *search2 = 0;
switch ( axis )
{
case X_AXIS:
if ( dx <= 0 ) // JWR if we are to the left
{
search1 = mLeft; // JWR then search to the left
if ( -dx < radius ) // JWR if distance to the right is less than our
search radius, continue on the right as well.
search2 = mRight;
}
else
{
search1 = mRight; // JWR ok, we go down the left tree
if ( dx < radius ) // JWR if the distance from the right is less than our
search radius
search2 = mLeft;
}
axis = Y_AXIS;
break;
case Y_AXIS:
if ( dy <= 0 )
{
search1 = mLeft;
if ( -dy < radius )
search2 = mRight;
}
else
{
search1 = mRight;
if ( dy < radius )
search2 = mLeft;
}
axis = Z_AXIS;
break;
case Z_AXIS:
if ( dz <= 0 )
{
search1 = mLeft;
if ( -dz < radius )
search2 = mRight;
}
else
{
search1 = mRight;
if ( dz < radius )
search2 = mLeft;
}
axis = X_AXIS;
break;
}
float r2 = radius*radius;
float m = dx*dx+dy*dy+dz*dz;
if ( m < r2 )
{
switch ( count )
{
case 0:
found[count].mNode = this;
found[count].mDistance = m;
break;
case 1:
if ( m < found[0].mDistance )
{
if ( maxObjects == 1 )
{
found[0].mNode = this;
found[0].mDistance = m;
}
else
{
found[1] = found[0];
found[0].mNode = this;
found[0].mDistance = m;
}
}
else if ( maxObjects > 1)
{
found[1].mNode = this;
found[1].mDistance = m;
}
break;
default:
{
bool inserted = false;
for (size_t i=0; i<count; i++)
{
if ( m < found[i].mDistance ) // if this one is closer than a pre-
existing one...
{
// insertion sort...
size_t scan = count;
if ( scan >= maxObjects ) scan=maxObjects-1;
for (size_t j=scan; j>i; j--)
{
found[j] = found[j-1];
}
found[i].mNode = this;
found[i].mDistance = m;
inserted = true;
break;
}
}
if ( !inserted && count < maxObjects )
{
found[count].mNode = this;
found[count].mDistance = m;
}
}
break;
}
count++;
if ( count > maxObjects )
{
count = maxObjects;
}
}
if ( search1 )
search1->search( axis, pos,radius, count, maxObjects, found, iface);
if ( search2 )
search2->search( axis, pos,radius, count, maxObjects, found, iface);
private:
void setLeft(KdTreeNode *left) { mLeft = left; };
void setRight(KdTreeNode *right) { mRight = right; };
KdTreeNode *getLeft(void) { return mLeft; }
KdTreeNode *getRight(void) { return mRight; }
size_t mIndex;
KdTreeNode *mLeft;
KdTreeNode *mRight;
};
#define MAX_BUNDLE_SIZE 1024 // 1024 nodes at a time, to minimize memory
allocation and guarentee that pointers are persistent.
class KdTreeNodeBundle
{
public:
KdTreeNodeBundle(void)
{
mNext = 0;
mIndex = 0;
}
bool isFull(void) const
{
return (bool)( mIndex == MAX_BUNDLE_SIZE );
}
KdTreeNode * getNextNode(void)
{
assert(mIndex<MAX_BUNDLE_SIZE);
KdTreeNode *ret = &mNodes[mIndex];
mIndex++;
return ret;
}
KdTreeNodeBundle *mNext;
size_t mIndex;
KdTreeNode mNodes[MAX_BUNDLE_SIZE];
};
typedef USER_STL::vector< double > DoubleVector;
typedef USER_STL::vector< float > FloatVector;
class KdTree : public KdTreeInterface
{
public:
KdTree(void)
{
mRoot = 0;
mBundle = 0;
mVcount = 0;
mUseDouble = false;
}
~KdTree(void)
{
reset();
}
const double * getPositionDouble(size_t index) const
{
assert( mUseDouble );
assert ( index < mVcount );
return &mVerticesDouble[index*3];
}
const float * getPositionFloat(size_t index) const
{
assert( !mUseDouble );
assert ( index < mVcount );
return &mVerticesFloat[index*3];
}
size_t search(const double *pos,double radius,size_t maxObjects,KdTreeFindNode
*found) const
{
assert( mUseDouble );
if ( !mRoot ) return 0;
size_t count = 0;
mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);
return count;
}
size_t search(const float *pos,float radius,size_t maxObjects,KdTreeFindNode
*found) const
{
assert( !mUseDouble );
if ( !mRoot ) return 0;
size_t count = 0;
mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);
return count;
}
void reset(void)
{
mRoot = 0;
mVerticesDouble.clear();
mVerticesFloat.clear();
KdTreeNodeBundle *bundle = mBundle;
while ( bundle )
{
KdTreeNodeBundle *next = bundle->mNext;
delete bundle;
bundle = next;
}
mBundle = 0;
mVcount = 0;
}
size_t add(double x,double y,double z)
{
assert(mUseDouble);
size_t ret = mVcount;
mVerticesDouble.push_back(x);
mVerticesDouble.push_back(y);
mVerticesDouble.push_back(z);
mVcount++;
KdTreeNode *node = getNewNode(ret);
if ( mRoot )
{
mRoot->addDouble(node,X_AXIS,this);
}
else
{
mRoot = node;
}
return ret;
}
size_t add(float x,float y,float z)
{
assert(!mUseDouble);
size_t ret = mVcount;
mVerticesFloat.push_back(x);
mVerticesFloat.push_back(y);
mVerticesFloat.push_back(z);
mVcount++;
KdTreeNode *node = getNewNode(ret);
if ( mRoot )
{
mRoot->addFloat(node,X_AXIS,this);
}
else
{
mRoot = node;
}
return ret;
}
KdTreeNode * getNewNode(size_t index)
{
if ( mBundle == 0 )
{
mBundle = MEMALLOC_NEW(KdTreeNodeBundle);
}
if ( mBundle->isFull() )
{
KdTreeNodeBundle *bundle = MEMALLOC_NEW(KdTreeNodeBundle);
mBundle->mNext = bundle;
mBundle = bundle;
}
KdTreeNode *node = mBundle->getNextNode();
new ( node ) KdTreeNode(index);
return node;
}
size_t getNearest(const double *pos,double radius,bool &_found) const // returns
the nearest possible neighbor's index.
{
assert( mUseDouble );
size_t ret = 0;
_found = false;
KdTreeFindNode found[1];
size_t count = search(pos,radius,1,found);
if ( count )
{
KdTreeNode *node = found[0].mNode;
ret = node->getIndex();
_found = true;
}
return ret;
}
size_t getNearest(const float *pos,float radius,bool &_found) const // returns
the nearest possible neighbor's index.
{
assert( !mUseDouble );
size_t ret = 0;
_found = false;
KdTreeFindNode found[1];
size_t count = search(pos,radius,1,found);
if ( count )
{
KdTreeNode *node = found[0].mNode;
ret = node->getIndex();
_found = true;
}
return ret;
}
const double * getVerticesDouble(void) const
{
assert( mUseDouble );
const double *ret = 0;
if ( !mVerticesDouble.empty() )
{
ret = &mVerticesDouble[0];
}
return ret;
}
const float * getVerticesFloat(void) const
{
assert( !mUseDouble );
const float * ret = 0;
if ( !mVerticesFloat.empty() )
{
ret = &mVerticesFloat[0];
}
return ret;
}
size_t getVcount(void) const { return mVcount; };
void setUseDouble(bool useDouble)
{
mUseDouble = useDouble;
}
private:
bool mUseDouble;
KdTreeNode *mRoot;
KdTreeNodeBundle *mBundle;
size_t mVcount;
DoubleVector mVerticesDouble;
FloatVector mVerticesFloat;
};
}; // end of namespace VERTEX_INDEX
class MyVertexIndex : public fm_VertexIndex
{
public:
MyVertexIndex(double granularity,bool snapToGrid)
{
mDoubleGranularity = granularity;
mFloatGranularity = (float)granularity;
mSnapToGrid = snapToGrid;
mUseDouble = true;
mKdTree.setUseDouble(true);
}
MyVertexIndex(float granularity,bool snapToGrid)
{
mDoubleGranularity = granularity;
mFloatGranularity = (float)granularity;
mSnapToGrid = snapToGrid;
mUseDouble = false;
mKdTree.setUseDouble(false);
}
double snapToGrid(double p)
{
double m = fmod(p,mDoubleGranularity);
p-=m;
return p;
}
float snapToGrid(float p)
{
float m = fmodf(p,mFloatGranularity);
p-=m;
return p;
}
size_t getIndex(const float *_p,bool &newPos) // get index for a vector float
{
size_t ret;
if ( mUseDouble )
{
double p[3];
p[0] = _p[0];
p[1] = _p[1];
p[2] = _p[2];
return getIndex(p,newPos);
}
newPos = false;
float p[3];
if ( mSnapToGrid )
{
p[0] = snapToGrid(_p[0]);
p[1] = snapToGrid(_p[1]);
p[2] = snapToGrid(_p[2]);
}
else
{
p[0] = _p[0];
p[1] = _p[1];
p[2] = _p[2];
}
bool found;
ret = mKdTree.getNearest(p,mFloatGranularity,found);
if ( !found )
{
newPos = true;
ret = mKdTree.add(p[0],p[1],p[2]);
}
return ret;
}
size_t getIndex(const double *_p,bool &newPos) // get index for a vector
double
{
size_t ret;
if ( !mUseDouble )
{
float p[3];
p[0] = (float)_p[0];
p[1] = (float)_p[1];
p[2] = (float)_p[2];
return getIndex(p,newPos);
}
newPos = false;
double p[3];
if ( mSnapToGrid )
{
p[0] = snapToGrid(_p[0]);
p[1] = snapToGrid(_p[1]);
p[2] = snapToGrid(_p[2]);
}
else
{
p[0] = _p[0];
p[1] = _p[1];
p[2] = _p[2];
}
bool found;
ret = mKdTree.getNearest(p,mDoubleGranularity,found);
if ( !found )
{
newPos = true;
ret = mKdTree.add(p[0],p[1],p[2]);
}
return ret;
}
const float * getVerticesFloat(void) const
{
const float * ret = 0;
assert( !mUseDouble );
ret = mKdTree.getVerticesFloat();
return ret;
}
const double * getVerticesDouble(void) const
{
const double * ret = 0;
assert( mUseDouble );
ret = mKdTree.getVerticesDouble();
return ret;
}
const float * getVertexFloat(size_t index) const
{
const float * ret = 0;
assert( !mUseDouble );
#ifdef _DEBUG
size_t vcount = mKdTree.getVcount();
assert( index < vcount );
#endif
ret = mKdTree.getVerticesFloat();
ret = &ret[index*3];
return ret;
}
const double * getVertexDouble(size_t index) const
{
const double * ret = 0;
assert( mUseDouble );
#ifdef _DEBUG
size_t vcount = mKdTree.getVcount();
assert( index < vcount );
#endif
ret = mKdTree.getVerticesDouble();
ret = &ret[index*3];
return ret;
}
size_t getVcount(void) const
{
return mKdTree.getVcount();
}
bool isDouble(void) const
{
return mUseDouble;
}
bool saveAsObj(const char *fname,unsigned int tcount,unsigned int
*indices)
{
bool ret = false;
FILE *fph = fopen(fname,"wb");
if ( fph )
{
ret = true;
size_t vcount = getVcount();
if ( mUseDouble )
{
const double *v = getVerticesDouble();
for (unsigned int i=0; i<vcount; i++)
{
fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", (float)v[0], (float)v[1],
(float)v[2] );
v+=3;
}
}
else
{
const float *v = getVerticesFloat();
for (unsigned int i=0; i<vcount; i++)
{
fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", v[0], v[1], v[2] );
v+=3;
}
}
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *indices++;
unsigned int i2 = *indices++;
unsigned int i3 = *indices++;
fprintf(fph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 );
}
fclose(fph);
}
return ret;
}
private:
bool mUseDouble:1;
bool mSnapToGrid:1;
double mDoubleGranularity;
float mFloatGranularity;
VERTEX_INDEX::KdTree mKdTree;
};
fm_VertexIndex * fm_createVertexIndex(double granularity,bool snapToGrid) // create
an indexed vertex system for doubles
{
MyVertexIndex *ret = MEMALLOC_NEW(MyVertexIndex)(granularity,snapToGrid);
return static_cast< fm_VertexIndex *>(ret);
}
fm_VertexIndex * fm_createVertexIndex(float granularity,bool snapToGrid) // create
an indexed vertext system for floats
{
MyVertexIndex *ret = MEMALLOC_NEW(MyVertexIndex)(granularity,snapToGrid);
return static_cast< fm_VertexIndex *>(ret);
}
void fm_releaseVertexIndex(fm_VertexIndex *vindex)
{
MyVertexIndex *m = static_cast< MyVertexIndex *>(vindex);
delete m;
}
#endif // END OF VERTEX WELDING CODE
//**********************************************************
//**********************************************************
//**** LineSweep Line-Segment Intersection Code
//**********************************************************
//**********************************************************
#ifndef LINE_SWEEP_H
#define LINE_SWEEP_H
#include <list>
#include <vector>
class fm_quickSort
{
public:
void qsort(void **base,int num); // perform the qsort.
protected:
// -1 less, 0 equal, +1 greater.
virtual int compare(void **p1,void **p2) = 0;
private:
void inline swap(char **a,char **b);
};
void fm_quickSort::swap(char **a,char **b)
{
char *tmp;
if ( a != b )
{
tmp = *a;
*a++ = *b;
*b++ = tmp;
}
}
void fm_quickSort::qsort(void **b,int num)
{
char *lo,*hi;
char *mid;
char *bottom, *top;
int size;
char *lostk[30], *histk[30];
int stkptr;
char **base = (char **)b;
if (num < 2 ) return;
stkptr = 0;
lo = (char *)base;
hi = (char *)base + sizeof(char **) * (num-1);
nextone:
size = (int)(hi - lo) / sizeof(char**) + 1;
mid = lo + (size / 2) * sizeof(char **);
swap((char **)mid,(char **)lo);
bottom = lo;
top = hi + sizeof(char **);
for (;;)
{
do
{
bottom += sizeof(char **);
} while (bottom <= hi && compare((void **)bottom,(void **)lo) <= 0);
do
{
top -= sizeof(char **);
} while (top > lo && compare((void **)top,(void **)lo) >= 0);
if (top < bottom) break;
swap((char **)bottom,(char **)top);
swap((char **)lo,(char **)top);
if ( top - 1 - lo >= hi - bottom )
{
if (lo + sizeof(char **) < top)
{
lostk[stkptr] = lo;
histk[stkptr] = top - sizeof(char **);
stkptr++;
}
if (bottom < hi)
{
lo = bottom;
goto nextone;
}
}
else
{
if ( bottom < hi )
{
lostk[stkptr] = bottom;
histk[stkptr] = hi;
stkptr++;
}
if (lo + sizeof(char **) < top)
{
hi = top - sizeof(char **);
goto nextone; /* do small recursion */
}
}
stkptr--;
if (stkptr >= 0)
{
lo = lostk[stkptr];
hi = histk[stkptr];
goto nextone;
}
return;
}
typedef USER_STL::vector< fm_LineSegment > LineSegmentVector;
static inline void setMinMax(double &vmin,double &vmax,double v1,double v2)
{
if ( v1 <= v2 )
{
vmin = v1;
vmax = v2;
}
else
{
vmin = v2;
vmax = v1;
}
}
class Intersection
{
public:
Intersection(void)
{
mIndex = 0;
mTime = 0;
}
Intersection(double time,const double *from,const double *to,fm_VertexIndex
*vpool)
{
mTime = time;
double pos[3];
pos[0] = (to[0]-from[0])*time+from[0];
pos[1] = (to[1]-from[1])*time+from[1];
pos[2] = (to[2]-from[2])*time+from[2];
bool newPos;
mIndex = vpool->getIndex(pos,newPos);
}
size_t mIndex;
double mTime;
};
typedef USER_STL::list< Intersection > IntersectionList;
class MyLineSegment : public fm_LineSegment
{
public:
void init(const fm_LineSegment &s,fm_VertexIndex *vpool,size_t x)
{
fm_LineSegment *dest = static_cast< fm_LineSegment *>(this);
*dest = s;
mFlipped = false;
const double *p1 = vpool->getVertexDouble(mE1);
const double *p2 = vpool->getVertexDouble(mE2);
setMinMax(mMin[0],mMax[0],p1[0],p2[0]);
setMinMax(mMin[1],mMax[1],p1[1],p2[1]);
setMinMax(mMin[2],mMax[2],p1[2],p2[2]);
if ( p1[x] <= p2[x] )
{
mFrom[0] = p1[0];
mFrom[1] = p1[1];
mFrom[2] = p1[2];
mTo[0] = p2[0];
mTo[1] = p2[1];
mTo[2] = p2[2];
}
else
{
mFrom[0] = p2[0];
mFrom[1] = p2[1];
mFrom[2] = p2[2];
mTo[0] = p1[0];
mTo[1] = p1[1];
mTo[2] = p1[2];
mFlipped = true;
swap(mE1,mE2);
}
// we already know that the x-extent overlaps or we wouldn't be in this routine..
void intersect(MyLineSegment *segment,size_t x,size_t y,size_t /* z
*/,fm_VertexIndex *vpool)
{
size_t count = 0;
// if the two segments share any start/end points then they cannot intersect at
all!
if ( segment->mE1 == mE1 || segment->mE1 == mE2 ) count++;
if ( segment->mE2 == mE1 || segment->mE2 == mE2 ) count++;
if ( count == 0 )
{
if ( mMax[y] < segment->mMin[y] ) // no intersection...
{
}
else if ( mMin[y] > segment->mMax[y] ) // no intersection
{
}
else
{
double a1[2];
double a2[2];
double b1[2];
double b2[2];
a1[0] = mFrom[x];
a1[1] = mFrom[y];
a2[0] = mTo[x];
a2[1] = mTo[y];
b1[0] = segment->mFrom[x];
b1[1] = segment->mFrom[y];
b2[0] = segment->mTo[x];
b2[1] = segment->mTo[y];
double t1,t2;
IntersectResult result = fm_intersectLineSegments2dTime(a1,a2,b1,b2,t1,t2);
if ( result == IR_DO_INTERSECT )
{
addIntersect(t1,vpool);
segment->addIntersect(t2,vpool);
}
}
}
}
void addIntersect(double time,fm_VertexIndex *vpool)
{
Intersection intersect(time,mFrom,mTo,vpool);
if ( mE1 == intersect.mIndex || mE2 == intersect.mIndex )
{
//printf("Split too close to the beginning or the end of the line segment.\r\
n");
}
else
{
if ( mIntersections.empty() )
{
mIntersections.push_back(intersect);
}
else
{
IntersectionList::iterator i;
for (i=mIntersections.begin(); i!=mIntersections.end(); ++i)
{
Intersection &it = (*i);
if ( it.mIndex == intersect.mIndex )
{
//printf("Duplicate Intersection, throwing it away.\r\n");
break;
}
else
{
if ( it.mTime > time )
{
mIntersections.insert(i,intersect);
break;
}
}
}
if ( i==mIntersections.end() )
{
mIntersections.push_back(intersect);
}
}
}
}
void getResults(LineSegmentVector &results)
{
if ( mIntersections.empty() )
{
fm_LineSegment seg(mE1,mE2);
if ( mFlipped )
{
swap(seg.mE1,seg.mE2);
}
results.push_back(seg);
}
else
{
size_t prev = mE1;
IntersectionList::iterator i;
for (i=mIntersections.begin(); i!=mIntersections.end(); ++i)
{
Intersection &it = (*i);
fm_LineSegment seg(prev,it.mIndex);
if ( mFlipped )
{
swap(seg.mE1,seg.mE2);
}
results.push_back(seg);
prev = it.mIndex;
}
fm_LineSegment seg(prev,mE2);
if ( mFlipped )
{
swap(seg.mE1,seg.mE2);
}
results.push_back(seg);
}
}
void swap(size_t &a,size_t &b)
{
size_t temp = a;
a = b;
b = temp;
}
bool mFlipped;
double mFrom[3];
double mTo[3];
double mMin[3];
double mMax[3];
IntersectionList mIntersections;
};
typedef USER_STL::vector< MyLineSegment > MyLineSegmentVector;
class MyLineSweep : public fm_LineSweep, public fm_quickSort
{
public:
fm_LineSegment * performLineSweep(const fm_LineSegment *segments,size_t
icount,const double *planeEquation,fm_VertexIndex *pool,size_t &scount)
{
fm_LineSegment *ret = 0;
FM_Axis axis = fm_getDominantAxis(planeEquation);
switch ( axis )
{
case FM_XAXIS:
mX = 1;
mY = 2;
mZ = 0;
break;
case FM_YAXIS:
mX = 0;
mY = 2;
mZ = 1;
break;
case FM_ZAXIS:
mX = 0;
mY = 1;
mZ = 2;
break;
}
mResults.clear();
scount = 0;
MyLineSegment *mls = MEMALLOC_NEW_ARRAY(MyLineSegment,icount)[icount];
MyLineSegment **mptr = MEMALLOC_NEW_ARRAY(MyLineSegment *,icount)[icount];
for (size_t i=0; i<icount; i++)
{
mls[i].init(segments[i],pool,mX);
mptr[i] = &mls[i];
}
qsort((void **)mptr,(int)icount);
for (size_t i=0; i<icount; i++)
{
MyLineSegment *segment = mptr[i];
double esegment = segment->mTo[mX];
for (size_t j=i+1; j<icount; j++)
{
MyLineSegment *test = mptr[j];
if ( test->mFrom[mX] >= esegment )
{
break;
}
else
{
test->intersect(segment,mX,mY,mZ,pool);
}
}
}
for (size_t i=0; i<icount; i++)
{
MyLineSegment *segment = mptr[i];
segment->getResults(mResults);
}
delete []mls;
delete []mptr;
if ( !mResults.empty() )
{
scount = mResults.size();
ret = &mResults[0];
}
return ret;
}
int compare(void **p1,void **p2)
{
int ret = 0;
MyLineSegment **m1 = (MyLineSegment **) p1;
MyLineSegment **m2 = (MyLineSegment **) p2;
MyLineSegment *s1 = *m1;
MyLineSegment *s2 = *m2;
if ( s1->mFrom[mX] < s2->mFrom[mX] )
ret = -1;
else if ( s1->mFrom[mX] > s2->mFrom[mX] )
ret = 1;
else if ( s1->mFrom[mY] < s2->mFrom[mY] )
ret = -1;
else if ( s1->mFrom[mY] > s2->mFrom[mY] )
ret = 1;
return ret;
}
size_t mX; // index for the x-axis
size_t mY; // index for the y-axis
size_t mZ;
fm_VertexIndex *mfm_VertexIndex;
LineSegmentVector mResults;
};
fm_LineSweep * fm_createLineSweep(void)
{
MyLineSweep *mls = MEMALLOC_NEW(MyLineSweep);
return static_cast< fm_LineSweep *>(mls);
}
void fm_releaseLineSweep(fm_LineSweep *sweep)
{
MyLineSweep *mls = static_cast< MyLineSweep *>(sweep);
delete mls;
}
#endif // End of LineSweep code
//**********************************************************
//**********************************************************
//**** Triangulation Code!
//**********************************************************
//**********************************************************
#ifndef TRIANGULATE_H
#define TRIANGULATE_H
static const double EPSILON=0.00000000000001;
typedef USER_STL::vector< size_t > size_tVector;
typedef USER_STL::vector< double > doubleVector;
typedef USER_STL::vector< float > floatVector;
class Vector2d
{
public:
Vector2d(float x,float y)
{
Set(x,y);
};
Vector2d(double x,double y)
{
Set(x,y);
}
Vector2d(const float *p)
{
mX = p[0];
mY = p[1];
}
Vector2d(const double *p)
{
mX = p[0];
mY = p[1];
}
double GetX(void) const { return mX; };
double GetY(void) const { return mY; };
void Set(double x,double y)
{
mX = x;
mY = y;
};
//private:
double mX;
double mY;
};
// Typedef an STL vector of vertices which are used to represent
// a polygon/contour and a series of triangles.
typedef USER_STL::vector< Vector2d > Vector2dVector;
class MyTriangulate : public fm_Triangulate
{
public:
MyTriangulate(void)
{
}
~MyTriangulate(void)
{
}
/*
InsideTriangle decides if a point P is Inside of the triangle
defined by A, B, C.
*/
bool Snip(const Vector2dVector &contour,int u,int v,int w,int n,int *V)
{
int p;
double Ax, Ay, Bx, By, Cx, Cy, Px, Py;
Ax = contour[V[u]].GetX();
Ay = contour[V[u]].GetY();
Bx = contour[V[v]].GetX();
By = contour[V[v]].GetY();
Cx = contour[V[w]].GetX();
Cy = contour[V[w]].GetY();
if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false;
for (p=0;p<n;p++)
{
if( (p == u) || (p == v) || (p == w) ) continue;
Px = contour[V[p]].GetX();
Py = contour[V[p]].GetY();
if (fm_insideTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false;
}
return true;
}
bool Process(const Vector2dVector &contour,size_tVector &result,bool &flipped)
{
/* allocate and initialize list of Vertices in polygon */
int n = (int)contour.size();
if ( n < 3 ) return false;
int *V = MEMALLOC_NEW_ARRAY(int,n)[n];
/* we want a counter-clockwise polygon in V */
if ( 0.0f < fm_areaPolygon2d(contour.size(),&contour[0].mX,sizeof(Vector2d)) )
{
for (int v=0; v<n; v++) V[v] = v;
flipped = false;
}
else
{
for(int v=0; v<n; v++) V[v] = (n-1)-v;
flipped = true;
}
int nv = n;
/* remove nv-2 Vertices, creating 1 triangle every time */
int count = 2*nv; /* error detection */
for(int m=0, v=nv-1; nv>2; )
{
/* if we loop, it is probably a non-simple polygon */
if (0 >= (count--))
{
//** Triangulate: ERROR - probable bad polygon!
MEMALLOC_DELETE_ARRAY(int *,V);
return false;
}
/* three consecutive vertices in current polygon, <u,v,w> */
int u = v;
if (nv <= u) u = 0; /* previous */
v = u+1;
if (nv <= v) v = 0; /* new v */
int w = v+1;
if (nv <= w) w = 0; /* next */
if ( Snip(contour,u,v,w,nv,V) )
{
int a,b,c,s,t;
/* true names of the vertices */
a = V[u];
b = V[v];
c = V[w];
/* output Triangle */
result.push_back( a );
result.push_back( b );
result.push_back( c );
m++;
/* remove v from remaining polygon */
for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--;
/* resest error detection counter */
count = 2*nv;
}
}
MEMALLOC_DELETE_ARRAY(int *,V);
return true;
}
void add(const double *p)
{
mTrianglesDouble.push_back(p[0]);
mTrianglesDouble.push_back(p[1]);
mTrianglesDouble.push_back(p[2]);
}
void add(const float *p)
{
mTrianglesFloat.push_back(p[0]);
mTrianglesFloat.push_back(p[1]);
mTrianglesFloat.push_back(p[2]);
}
const double * triangulate3d(size_t pcount,const double *_points,size_t
vstride,size_t &tcount)
{
const double *ret = 0;
mTrianglesDouble.clear();
tcount = 0;
if ( pcount >= 3 )
{
double *points = MEMALLOC_NEW_ARRAY(double,pcount*3)[pcount*3];
pcount = fm_consolidatePolygon(pcount,_points,vstride,points);
vstride = sizeof(double)*3;
if ( pcount >= 3 )
{
const double *p1 = fm_getPoint(points,vstride,0);
const double *p2 = fm_getPoint(points,vstride,1);
const double *p3 = fm_getPoint(points,vstride,2);
double plane[4];
plane[3] = fm_computePlane(p1,p2,p3,plane);
FM_Axis axis = fm_getDominantAxis(plane);
size_t ix = 0;
size_t iy = 0;
switch ( axis )
{
case FM_XAXIS:
ix = 1;
iy = 2;
break;
case FM_YAXIS:
ix = 0;
iy = 2;
break;
case FM_ZAXIS:
ix = 0;
iy = 1;
break;
}
Vector2dVector polygon;
unsigned char *scan = (unsigned char *)points;
for (size_t i=0; i<pcount; i++)
{
const double *v = (const double *)scan;
Vector2d point( v[ix], v[iy] );
polygon.push_back(point);
scan+=vstride;
}
bool flipped;
size_tVector results;
if ( Process(polygon,results,flipped) && !results.empty() )
{
size_t tcount = results.size()/3;
size_t *indices = &results[0];
for (size_t i=0; i<tcount; i++,indices+=3)
{
size_t i1 = indices[0];
size_t i2 = indices[1];
size_t i3 = indices[2];
const double *p1 = fm_getPoint(points,vstride,i1);
const double *p2 = fm_getPoint(points,vstride,i2);
const double *p3 = fm_getPoint(points,vstride,i3);
if ( flipped )
{
add(p1);
add(p2);
add(p3);
}
else
{
add(p3);
add(p2);
add(p1);
}
}
}
}
delete []points;
}
tcount = mTrianglesDouble.size()/9;
if ( tcount )
{
ret = &mTrianglesDouble[0];
}
return ret;
}
const float * triangulate3d(size_t pcount,const float *points,size_t
vstride,size_t &tcount)
{
const float *ret = 0;
mTrianglesFloat.clear();
doubleVector polygon;
for (size_t i=0; i<pcount; i++)
{
const float *p = fm_getPoint(points,vstride,i);
double dp[3];
fm_floatToDouble3(p,dp);
polygon.push_back(dp[0]);
polygon.push_back(dp[1]);
polygon.push_back(dp[2]);
}
const double *results =
triangulate3d(pcount,&polygon[0],sizeof(double)*3,tcount);
for (size_t i=0; i<tcount*9; i++)
{
float v = (float) *results++;
mTrianglesFloat.push_back(v);
}
ret = &mTrianglesFloat[0];
return ret;
}
doubleVector mTrianglesDouble;
floatVector mTrianglesFloat;
};
fm_Triangulate * fm_createTriangulate(void)
{
MyTriangulate *mt = MEMALLOC_NEW(MyTriangulate);
return static_cast< fm_Triangulate *>(mt);
}
void fm_releaseTriangulate(fm_Triangulate *t)
{
MyTriangulate *mt = static_cast< MyTriangulate *>(t);
delete mt;
}
#endif // End of Triangulation code
REAL fm_computeBestFitAABB(size_t vcount,const REAL *points,size_t pstride,REAL
*bmin,REAL *bmax) // returns the diagonal distance
{
const unsigned char *source = (const unsigned char *) points;
bmin[0] = points[0];
bmin[1] = points[1];
bmin[2] = points[2];
bmax[0] = points[0];
bmax[1] = points[1];
bmax[2] = points[2];
for (size_t i=1; i<vcount; i++)
{
source+=pstride;
const REAL *p = (const REAL *) source;
if ( p[0] < bmin[0] ) bmin[0] = p[0];
if ( p[1] < bmin[1] ) bmin[1] = p[1];
if ( p[2] < bmin[2] ) bmin[2] = p[2];
if ( p[0] > bmax[0] ) bmax[0] = p[0];
if ( p[1] > bmax[1] ) bmax[1] = p[1];
if ( p[2] > bmax[2] ) bmax[2] = p[2];
REAL dx = bmax[0] - bmin[0];
REAL dy = bmax[1] - bmin[1];
REAL dz = bmax[2] - bmin[2];
return (REAL) sqrt( dx*dx + dy*dy + dz*dz );
}
/* a = b - c */
#define vector(a,b,c) \
(a)[0] = (b)[0] - (c)[0]; \
(a)[1] = (b)[1] - (c)[1]; \
(a)[2] = (b)[2] - (c)[2];
#define innerProduct(v,q) \
((v)[0] * (q)[0] + \
(v)[1] * (q)[1] + \
(v)[2] * (q)[2])
#define crossProduct(a,b,c) \
(a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
(a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
(a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
bool fm_lineIntersectsTriangle(const REAL *rayStart,const REAL *rayEnd,const REAL
*p1,const REAL *p2,const REAL *p3,REAL *sect)
{
REAL dir[3];
dir[0] = rayEnd[0] - rayStart[0];
dir[1] = rayEnd[1] - rayStart[1];
dir[2] = rayEnd[2] - rayStart[2];
REAL d = (REAL)sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
REAL r = 1.0f / d;
dir[0]*=r;
dir[1]*=r;
dir[2]*=r;
REAL t;
bool ret = fm_rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
if ( ret )
{
if ( t > d )
{
sect[0] = rayStart[0] + dir[0]*t;
sect[1] = rayStart[1] + dir[1]*t;
sect[2] = rayStart[2] + dir[2]*t;
}
else
{
ret = false;
}
}
return ret;
}
bool fm_rayIntersectsTriangle(const REAL *p,const REAL *d,const REAL *v0,const REAL
*v1,const REAL *v2,REAL &t)
{
REAL e1[3],e2[3],h[3],s[3],q[3];
REAL a,f,u,v;
vector(e1,v1,v0);
vector(e2,v2,v0);
crossProduct(h,d,e2);
a = innerProduct(e1,h);
if (a > -0.00001 && a < 0.00001)
return(false);
f = 1/a;
vector(s,p,v0);
u = f * (innerProduct(s,h));
if (u < 0.0 || u > 1.0)
return(false);
crossProduct(q,s,e1);
v = f * innerProduct(d,q);
if (v < 0.0 || u + v > 1.0)
return(false);
// at this stage we can compute t to find out where
// the intersection point is on the line
t = f * innerProduct(e2,q);
if (t > 0) // ray intersection
return(true);
else // this means that there is a line intersection
// but not a ray intersection
return (false);
}
inline REAL det(const REAL *p1,const REAL *p2,const REAL *p3)
{
return p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -
p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
}
REAL fm_computeMeshVolume(const REAL *vertices,size_t tcount,const unsigned int
*indices)
{
REAL volume = 0;
for (unsigned int i=0; i<tcount; i++,indices+=3)
{
const REAL *p1 = &vertices[ indices[0]*3 ];
const REAL *p2 = &vertices[ indices[1]*3 ];
const REAL *p3 = &vertices[ indices[2]*3 ];
volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the
origin.
}
volume*=(1.0f/6.0f);
if ( volume < 0 )
volume*=-1;
return volume;
}
const REAL * fm_getPoint(const REAL *points,size_t pstride,size_t index)
{
const unsigned char *scan = (const unsigned char *)points;
scan+=(index*pstride);
return (REAL *)scan;
}
bool fm_insideTriangle(REAL Ax, REAL Ay,
REAL Bx, REAL By,
REAL Cx, REAL Cy,
REAL Px, REAL Py)
{
REAL ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
REAL cCROSSap, bCROSScp, aCROSSbp;
ax = Cx - Bx; ay = Cy - By;
bx = Ax - Cx; by = Ay - Cy;
cx = Bx - Ax; cy = By - Ay;
apx= Px - Ax; apy= Py - Ay;
bpx= Px - Bx; bpy= Py - By;
cpx= Px - Cx; cpy= Py - Cy;
aCROSSbp = ax*bpy - ay*bpx;
cCROSSap = cx*apy - cy*apx;
bCROSScp = bx*cpy - by*cpx;
return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
}
REAL fm_areaPolygon2d(size_t pcount,const REAL *points,size_t pstride)
{
int n = (int)pcount;
REAL A=0.0f;
for(int p=n-1,q=0; q<n; p=q++)
{
const REAL *p1 = fm_getPoint(points,pstride,p);
const REAL *p2 = fm_getPoint(points,pstride,q);
A+= p1[0]*p2[1] - p2[0]*p1[1];
}
return A*0.5f;
}
bool fm_pointInsidePolygon2d(size_t pcount,const REAL *points,size_t pstride,const
REAL *point,size_t xindex,size_t yindex)
{
size_t j = pcount-1;
int oddNodes = 0;
REAL x = point[xindex];
REAL y = point[yindex];
for (size_t i=0; i<pcount; i++)
{
const REAL *p1 = fm_getPoint(points,pstride,i);
const REAL *p2 = fm_getPoint(points,pstride,j);
REAL x1 = p1[xindex];
REAL y1 = p1[yindex];
REAL x2 = p2[xindex];
REAL y2 = p2[yindex];
if ( y1 < y && y2 >= y || y2 < y && y1 >= y )
{
if (x1+(y-y1)/(y2-y1)*(x2-x1)<x)
{
oddNodes = 1-oddNodes;
}
}
j = i;
}
return oddNodes ? true : false;
}
size_t fm_consolidatePolygon(size_t pcount,const REAL *points,size_t pstride,REAL
*_dest,REAL epsilon) // collapses co-linear edges.
{
size_t ret = 0;
if ( pcount >= 3 )
{
const REAL *prev = fm_getPoint(points,pstride,pcount-1);
const REAL *current = points;
const REAL *next = fm_getPoint(points,pstride,1);
REAL *dest = _dest;
for (size_t i=0; i<pcount; i++)
{
next = (i+1)==pcount ? points : next;
if ( !fm_colinear(prev,current,next,epsilon) )
{
dest[0] = current[0];
dest[1] = current[1];
dest[2] = current[2];
dest+=3;
ret++;
}
prev = current;
current+=3;
next+=3;
}
}
return ret;
}
#ifndef RECT3D_TEMPLATE
#define RECT3D_TEMPLATE
template <class T> class Rect3d
{
public:
Rect3d(void) { };
Rect3d(const T *bmin,const T *bmax)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
void SetMin(const T *bmin)
{
mMin[0] = bmin[0];
mMin[1] = bmin[1];
mMin[2] = bmin[2];
}
void SetMax(const T *bmax)
{
mMax[0] = bmax[0];
mMax[1] = bmax[1];
mMax[2] = bmax[2];
}
void SetMin(T x,T y,T z)
{
mMin[0] = x;
mMin[1] = y;
mMin[2] = z;
}
void SetMax(T x,T y,T z)
{
mMax[0] = x;
mMax[1] = y;
mMax[2] = z;
}
T mMin[3];
T mMax[3];
};
#endif
void splitRect(unsigned int axis,
const Rect3d<REAL> &source,
Rect3d<REAL> &b1,
Rect3d<REAL> &b2,
const REAL *midpoint)
{
switch ( axis )
{
case 0:
b1.SetMin(source.mMin);
b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );
b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );
b2.SetMax(source.mMax);
break;
case 1:
b1.SetMin(source.mMin);
b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );
b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );
b2.SetMax(source.mMax);
break;
case 2:
b1.SetMin(source.mMin);
b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );
b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
b2.SetMax(source.mMax);
break;
}
}
bool fm_computeSplitPlane(unsigned int vcount,
const REAL *vertices,
unsigned int /* tcount */,
const unsigned int * /* indices */,
REAL *plane)
{
REAL sides[3];
REAL matrix[16];
fm_computeBestFitOBB( vcount, vertices, sizeof(REAL)*3, sides, matrix );
REAL bmax[3];
REAL bmin[3];
bmax[0] = sides[0]*0.5f;
bmax[1] = sides[1]*0.5f;
bmax[2] = sides[2]*0.5f;
bmin[0] = -bmax[0];
bmin[1] = -bmax[1];
bmin[2] = -bmax[2];
REAL dx = sides[0];
REAL dy = sides[1];
REAL dz = sides[2];
REAL laxis = dx;
unsigned int axis = 0;
if ( dy > dx )
{
axis = 1;
laxis = dy;
}
if ( dz > dx && dz > dy )
{
axis = 2;
laxis = dz;
}
REAL p1[3];
REAL p2[3];
REAL p3[3];
p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;
p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;
p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;
Rect3d<REAL> b(bmin,bmax);
Rect3d<REAL> b1,b2;
splitRect(axis,b,b1,b2,p1);
switch ( axis )
{
case 0:
p2[1] = bmin[1];
p2[2] = bmin[2];
if ( dz > dy )
{
p3[1] = bmax[1];
p3[2] = bmin[2];
}
else
{
p3[1] = bmin[1];
p3[2] = bmax[2];
}
break;
case 1:
p2[0] = bmin[0];
p2[2] = bmin[2];
if ( dx > dz )
{
p3[0] = bmax[0];
p3[2] = bmin[2];
}
else
{
p3[0] = bmin[0];
p3[2] = bmax[2];
}
break;
case 2:
p2[0] = bmin[0];
p2[1] = bmin[1];
if ( dx > dy )
{
p3[0] = bmax[0];
p3[1] = bmin[1];
}
else
{
p3[0] = bmin[0];
p3[1] = bmax[1];
}
break;
}
REAL tp1[3];
REAL tp2[3];
REAL tp3[3];
fm_transform(matrix,p1,tp1);
fm_transform(matrix,p2,tp2);
fm_transform(matrix,p3,tp3);
plane[3] = fm_computePlane(tp1,tp2,tp3,plane);
return true;
#pragma warning(disable:4100)
void fm_nearestPointInTriangle(const REAL *nearestPoint,const REAL *p1,const REAL
*p2,const REAL *p3,REAL *nearest)
{
}
static REAL Partial(const REAL *a,const REAL *p)
{
return (a[0]*p[1]) - (p[0]*a[1]);
}
REAL fm_areaTriangle(const REAL *p0,const REAL *p1,const REAL *p2)
{
REAL A = Partial(p0,p1);
A+= Partial(p1,p2);
A+= Partial(p2,p0);
return A*0.5f;
}
void fm_subtract(const REAL *A,const REAL *B,REAL *diff) // compute A-B and store
the result in 'diff'
{
diff[0] = A[0]-B[0];
diff[1] = A[1]-B[1];
diff[2] = A[2]-B[2];
}
void fm_multiplyTransform(const REAL *pA,const REAL *pB,REAL *pM)
{
REAL a = pA[0*4+0] * pB[0*4+0] + pA[0*4+1] * pB[1*4+0] + pA[0*4+2] * pB[2*4+0] +
pA[0*4+3] * pB[3*4+0];
REAL b = pA[0*4+0] * pB[0*4+1] + pA[0*4+1] * pB[1*4+1] + pA[0*4+2] * pB[2*4+1] +
pA[0*4+3] * pB[3*4+1];
REAL c = pA[0*4+0] * pB[0*4+2] + pA[0*4+1] * pB[1*4+2] + pA[0*4+2] * pB[2*4+2] +
pA[0*4+3] * pB[3*4+2];
REAL d = pA[0*4+0] * pB[0*4+3] + pA[0*4+1] * pB[1*4+3] + pA[0*4+2] * pB[2*4+3] +
pA[0*4+3] * pB[3*4+3];
REAL e = pA[1*4+0] * pB[0*4+0] + pA[1*4+1] * pB[1*4+0] + pA[1*4+2] * pB[2*4+0] +
pA[1*4+3] * pB[3*4+0];
REAL f = pA[1*4+0] * pB[0*4+1] + pA[1*4+1] * pB[1*4+1] + pA[1*4+2] * pB[2*4+1] +
pA[1*4+3] * pB[3*4+1];
REAL g = pA[1*4+0] * pB[0*4+2] + pA[1*4+1] * pB[1*4+2] + pA[1*4+2] * pB[2*4+2] +
pA[1*4+3] * pB[3*4+2];
REAL h = pA[1*4+0] * pB[0*4+3] + pA[1*4+1] * pB[1*4+3] + pA[1*4+2] * pB[2*4+3] +
pA[1*4+3] * pB[3*4+3];
REAL i = pA[2*4+0] * pB[0*4+0] + pA[2*4+1] * pB[1*4+0] + pA[2*4+2] * pB[2*4+0] +
pA[2*4+3] * pB[3*4+0];
REAL j = pA[2*4+0] * pB[0*4+1] + pA[2*4+1] * pB[1*4+1] + pA[2*4+2] * pB[2*4+1] +
pA[2*4+3] * pB[3*4+1];
REAL k = pA[2*4+0] * pB[0*4+2] + pA[2*4+1] * pB[1*4+2] + pA[2*4+2] * pB[2*4+2] +
pA[2*4+3] * pB[3*4+2];
REAL l = pA[2*4+0] * pB[0*4+3] + pA[2*4+1] * pB[1*4+3] + pA[2*4+2] * pB[2*4+3] +
pA[2*4+3] * pB[3*4+3];
REAL m = pA[3*4+0] * pB[0*4+0] + pA[3*4+1] * pB[1*4+0] + pA[3*4+2] * pB[2*4+0] +
pA[3*4+3] * pB[3*4+0];
REAL n = pA[3*4+0] * pB[0*4+1] + pA[3*4+1] * pB[1*4+1] + pA[3*4+2] * pB[2*4+1] +
pA[3*4+3] * pB[3*4+1];
REAL o = pA[3*4+0] * pB[0*4+2] + pA[3*4+1] * pB[1*4+2] + pA[3*4+2] * pB[2*4+2] +
pA[3*4+3] * pB[3*4+2];
REAL p = pA[3*4+0] * pB[0*4+3] + pA[3*4+1] * pB[1*4+3] + pA[3*4+2] * pB[2*4+3] +
pA[3*4+3] * pB[3*4+3];
pM[0] = a; pM[1] = b; pM[2] = c; pM[3] = d;
pM[4] = e; pM[5] = f; pM[6] = g; pM[7] = h;
pM[8] = i; pM[9] = j; pM[10] = k; pM[11] = l;
pM[12] = m; pM[13] = n; pM[14] = o; pM[15] = p;
}
void fm_multiply(REAL *A,REAL scaler)
{
A[0]*=scaler;
A[1]*=scaler;
A[2]*=scaler;
}
void fm_add(const REAL *A,const REAL *B,REAL *sum)
{
sum[0] = A[0]+B[0];
sum[1] = A[1]+B[1];
sum[2] = A[2]+B[2];
}
void fm_copy3(const REAL *source,REAL *dest)
{
dest[0] = source[0];
dest[1] = source[1];
dest[2] = source[2];
}
size_t fm_copyUniqueVertices(size_t vcount,const REAL *input_vertices,REAL
*output_vertices,size_t tcount,const size_t *input_indices,size_t *output_indices)
{
size_t ret = 0;
REAL *vertices = MEMALLOC_NEW_ARRAY(REAL,vcount*3)[vcount*3];
memcpy(vertices,input_vertices,sizeof(REAL)*vcount*3);
REAL *dest = output_vertices;
size_t *reindex = MEMALLOC_NEW_ARRAY(size_t,vcount)[vcount];
memset(reindex,0xFF,sizeof(size_t)*vcount);
size_t icount = tcount*3;
for (size_t i=0; i<icount; i++)
{
size_t index = *input_indices++;
assert( index < vcount );
if ( reindex[index] == 0xFFFFFFFF )
{
*output_indices++ = ret;
reindex[index] = ret;
const REAL *pos = &vertices[index*3];
dest[0] = pos[0];
dest[1] = pos[1];
dest[2] = pos[2];
dest+=3;
ret++;
}
else
{
*output_indices++ = reindex[index];
}
}
delete []vertices;
delete []reindex;
return ret;
}
bool fm_isMeshCoplanar(size_t tcount,const size_t *indices,const REAL
*vertices,bool doubleSided) // returns true if this collection of indexed triangles
are co-planar!
{
bool ret = true;
if ( tcount > 0 )
{
size_t i1 = indices[0];
size_t i2 = indices[1];
size_t i3 = indices[2];
const REAL *p1 = &vertices[i1*3];
const REAL *p2 = &vertices[i2*3];
const REAL *p3 = &vertices[i3*3];
REAL plane[4];
plane[3] = fm_computePlane(p1,p2,p3,plane);
const size_t *scan = &indices[3];
for (size_t i=1; i<tcount; i++)
{
i1 = *scan++;
i2 = *scan++;
i3 = *scan++;
p1 = &vertices[i1*3];
p2 = &vertices[i2*3];
p3 = &vertices[i3*3];
REAL _plane[4];
_plane[3] = fm_computePlane(p1,p2,p3,_plane);
if ( !fm_samePlane(plane,_plane,0.01f,0.001f,doubleSided) )
{
ret = false;
break;
}
}
}
return ret;
}
bool fm_samePlane(const REAL p1[4],const REAL p2[4],REAL normalEpsilon,REAL
dEpsilon,bool doubleSided)
{
bool ret = false;
REAL diff = (REAL) fabs(p1[3]-p2[3]);
if ( diff < dEpsilon ) // if the plane -d co-efficient is within our epsilon
{
REAL dot = fm_dot(p1,p2); // compute the dot-product of the vector normals.
if ( doubleSided ) dot = (REAL)fabs(dot);
REAL dmin = 1 - normalEpsilon;
REAL dmax = 1 + normalEpsilon;
if ( dot >= dmin && dot <= dmax )
{
ret = true; // then the plane equation is for practical purposes identical.
}
}
return ret;
}
void fm_initMinMax(REAL bmin[3],REAL bmax[3])
{
bmin[0] = FLT_MAX;
bmin[1] = FLT_MAX;
bmin[2] = FLT_MAX;
bmax[0] = FLT_MIN;
bmax[1] = FLT_MIN;
bmax[2] = FLT_MIN;
}
#ifndef TESSELATE_H
#define TESSELATE_H
typedef USER_STL::vector< size_t > size_tVector;
class Myfm_Tesselate : public fm_Tesselate
{
public:
const size_t * tesselate(fm_VertexIndex *vindex,size_t tcount,const size_t
*indices,float longEdge,size_t maxDepth,size_t &outcount)
{
const size_t *ret = 0;
mMaxDepth = maxDepth;
mLongEdge = longEdge*longEdge;
mLongEdgeD = mLongEdge;
mVertices = vindex;
if ( mVertices->isDouble() )
{
size_t vcount = mVertices->getVcount();
double *vertices = MEMALLOC_NEW_ARRAY(double,vcount*3)[vcount*3];
memcpy(vertices,mVertices->getVerticesDouble(),sizeof(double)*vcount*3);
for (size_t i=0; i<tcount; i++)
{
size_t i1 = *indices++;
size_t i2 = *indices++;
size_t i3 = *indices++;
const double *p1 = &vertices[i1*3];
const double *p2 = &vertices[i2*3];
const double *p3 = &vertices[i3*3];
tesselate(p1,p2,p3,0);
}
delete []vertices;
}
else
{
size_t vcount = mVertices->getVcount();
float *vertices = MEMALLOC_NEW_ARRAY(float,vcount*3)[vcount*3];
memcpy(vertices,mVertices->getVerticesFloat(),sizeof(float)*vcount*3);
for (size_t i=0; i<tcount; i++)
{
size_t i1 = *indices++;
size_t i2 = *indices++;
size_t i3 = *indices++;
const float *p1 = &vertices[i1*3];
const float *p2 = &vertices[i2*3];
const float *p3 = &vertices[i3*3];
tesselate(p1,p2,p3,0);
}
delete []vertices;
}
outcount = mIndices.size()/3;
ret = &mIndices[0];
return ret;
}
void tesselate(const float *p1,const float *p2,const float *p3,size_t recurse)
{
bool split = false;
float l1,l2,l3;
l1 = l2 = l3 = 0;
if ( recurse < mMaxDepth )
{
l1 = fm_distanceSquared(p1,p2);
l2 = fm_distanceSquared(p2,p3);
l3 = fm_distanceSquared(p3,p1);
if ( l1 > mLongEdge || l2 > mLongEdge || l3 > mLongEdge )
split = true;
if ( split )
{
size_t edge;
if ( l1 >= l2 && l1 >= l3 )
edge = 0;
else if ( l2 >= l1 && l2 >= l3 )
edge = 1;
else
edge = 2;
float split[3];
switch ( edge )
{
case 0:
{
fm_lerp(p1,p2,split,0.5f);
tesselate(p1,split,p3, recurse+1 );
tesselate(split,p2,p3, recurse+1 );
}
break;
case 1:
{
fm_lerp(p2,p3,split,0.5f);
tesselate(p1,p2,split, recurse+1 );
tesselate(p1,split,p3, recurse+1 );
}
break;
case 2:
{
fm_lerp(p3,p1,split,0.5f);
tesselate(p1,p2,split, recurse+1 );
tesselate(split,p2,p3, recurse+1 );
}
break;
}
}
else
{
bool newp;
size_t i1 = mVertices->getIndex(p1,newp);
size_t i2 = mVertices->getIndex(p2,newp);
size_t i3 = mVertices->getIndex(p3,newp);
mIndices.push_back(i1);
mIndices.push_back(i2);
mIndices.push_back(i3);
}
void tesselate(const double *p1,const double *p2,const double *p3,size_t recurse)
{
bool split = false;
double l1,l2,l3;
l1 = l2 = l3 = 0;
if ( recurse < mMaxDepth )
{
l1 = fm_distanceSquared(p1,p2);
l2 = fm_distanceSquared(p2,p3);
l3 = fm_distanceSquared(p3,p1);
if ( l1 > mLongEdgeD || l2 > mLongEdgeD || l3 > mLongEdgeD )
split = true;
if ( split )
{
size_t edge;
if ( l1 >= l2 && l1 >= l3 )
edge = 0;
else if ( l2 >= l1 && l2 >= l3 )
edge = 1;
else
edge = 2;
double split[3];
switch ( edge )
{
case 0:
{
fm_lerp(p1,p2,split,0.5);
tesselate(p1,split,p3, recurse+1 );
tesselate(split,p2,p3, recurse+1 );
}
break;
case 1:
{
fm_lerp(p2,p3,split,0.5);
tesselate(p1,p2,split, recurse+1 );
tesselate(p1,split,p3, recurse+1 );
}
break;
case 2:
{
fm_lerp(p3,p1,split,0.5);
tesselate(p1,p2,split, recurse+1 );
tesselate(split,p2,p3, recurse+1 );
}
break;
}
}
else
{
bool newp;
size_t i1 = mVertices->getIndex(p1,newp);
size_t i2 = mVertices->getIndex(p2,newp);
size_t i3 = mVertices->getIndex(p3,newp);
mIndices.push_back(i1);
mIndices.push_back(i2);
mIndices.push_back(i3);
}
private:
float mLongEdge;
double mLongEdgeD;
fm_VertexIndex *mVertices;
size_tVector mIndices;
size_t mMaxDepth;
};
fm_Tesselate * fm_createTesselate(void)
{
Myfm_Tesselate *m = MEMALLOC_NEW(Myfm_Tesselate);
return static_cast< fm_Tesselate * >(m);
}
void fm_releaseTesselate(fm_Tesselate *t)
{
Myfm_Tesselate *m = static_cast< Myfm_Tesselate *>(t);
delete m;
}
#endif
#ifndef RAY_ABB_INTERSECT
#define RAY_ABB_INTERSECT
//! Integer representation of a floating-point value.
#define IR(x) ((unsigned int&)x)
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////
/**
* A method to compute a ray-AABB intersection.
* Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
* Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
* Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
*
* Hence this version is faster as well as more robust than the original one.
*
* Should work provided:
* 1) the integer representation of 0.0f is 0x00000000
* 2) the sign bit of the float is the most significant one
*
* Report bugs: [email protected]
*
* \param aabb [in] the axis-aligned bounding box
* \param origin [in] ray origin
* \param dir [in] ray direction
* \param coord [out] impact coordinates
* \return true if ray intersects AABB
*/
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////
#define RAYAABB_EPSILON 0.00001f
bool fm_intersectRayAABB(const float MinB[3],const float MaxB[3],const float
origin[3],const float dir[3],float coord[3])
{
bool Inside = true;
float MaxT[3];
MaxT[0]=MaxT[1]=MaxT[2]=-1.0f;
// Find candidate planes.
for(unsigned int i=0;i<3;i++)
{
if(origin[i] < MinB[i])
{
coord[i] = MinB[i];
Inside = false;
// Calculate T distances to candidate planes
if(IR(dir[i])) MaxT[i] = (MinB[i] - origin[i]) / dir[i];
}
else if(origin[i] > MaxB[i])
{
coord[i] = MaxB[i];
Inside = false;
// Calculate T distances to candidate planes
if(IR(dir[i])) MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
}
}
// Ray origin inside bounding box
if(Inside)
{
coord[0] = origin[0];
coord[1] = origin[1];
coord[2] = origin[2];
return true;
}
// Get largest of the maxT's for final choice of intersection
unsigned int WhichPlane = 0;
if(MaxT[1] > MaxT[WhichPlane]) WhichPlane = 1;
if(MaxT[2] > MaxT[WhichPlane]) WhichPlane = 2;
// Check final candidate actually inside box
if(IR(MaxT[WhichPlane])&0x80000000) return false;
for(unsigned int i=0;i<3;i++)
{
if(i!=WhichPlane)
{
coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
#ifdef RAYAABB_EPSILON
if(coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] +
RAYAABB_EPSILON) return false;
#else
if(coord[i] < MinB[i] || coord[i] > MaxB[i]) return false;
#endif
}
}
return true; // ray hits box
}
bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const
float p1[3],const float p2[3],float intersect[3])
{
bool ret = false;
float dir[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = p2[2] - p1[2];
float dist = fm_normalize(dir);
if ( dist > RAYAABB_EPSILON )
{
ret = fm_intersectRayAABB(bmin,bmax,p1,dir,intersect);
if ( ret )
{
float d = fm_distanceSquared(p1,intersect);
if ( d > (dist*dist) )
{
ret = false;
}
}
}
return ret;
}
#endif
#ifndef OBB_TO_AABB
#define OBB_TO_AABB
#pragma warning(disable:4100)
void fm_OBBtoAABB(const float obmin[3],const float obmax[3],const float
matrix[16],float abmin[3],float abmax[3])
{
assert(0); // not yet implemented.
}
const REAL * computePos(unsigned int index,const REAL *vertices,unsigned int
vstride)
{
const char *tmp = (const char *)vertices;
tmp+=(index*vstride);
return (const REAL*)tmp;
}
void computeNormal(unsigned int index,REAL *normals,unsigned int nstride,const REAL
*normal)
{
char *tmp = (char *)normals;
tmp+=(index*nstride);
REAL *dest = (REAL *)tmp;
dest[0]+=normal[0];
dest[1]+=normal[1];
dest[2]+=normal[2];
}
void fm_computeMeanNormals(unsigned int vcount, // the number of vertices
const REAL *vertices, // the base address of the
vertex position data.
unsigned int vstride, // the stride between
position data.
REAL *normals, // the base address of the
destination for mean vector normals
unsigned int nstride, // the stride between normals
unsigned int tcount, // the number of triangles
const unsigned int *indices) // the triangle indices
{
// Step #1 : Zero out the vertex normals
char *dest = (char *)normals;
for (unsigned int i=0; i<vcount; i++)
{
REAL *n = (REAL *)dest;
n[0] = 0;
n[1] = 0;
n[2] = 0;
dest+=nstride;
}
// Step #2 : Compute the face normals and accumulate them
const unsigned int *scan = indices;
for (unsigned int i=0; i<tcount; i++)
{
unsigned int i1 = *scan++;
unsigned int i2 = *scan++;
unsigned int i3 = *scan++;
const REAL *p1 = computePos(i1,vertices,vstride);
const REAL *p2 = computePos(i2,vertices,vstride);
const REAL *p3 = computePos(i3,vertices,vstride);
REAL normal[3];
fm_computePlane(p3,p2,p1,normal);
computeNormal(i1,normals,nstride,normal);
computeNormal(i2,normals,nstride,normal);
computeNormal(i3,normals,nstride,normal);
}
// Normalize the accumulated normals
dest = (char *)normals;
for (unsigned int i=0; i<vcount; i++)
{
REAL *n = (REAL *)dest;
fm_normalize(n);
dest+=nstride;
}
}
#endif
void fm_computeBestFitCapsule(size_t vcount,const REAL *points,size_t pstride,REAL
&radius,REAL &height,REAL matrix[16],FitStrategy strategy)
{
REAL sides[3];
REAL omatrix[16];
fm_computeBestFitOBB(vcount,points,pstride,sides,omatrix,strategy);
int axis = 0;
if ( sides[0] > sides[1] && sides[0] > sides[2] )
axis = 0;
else if ( sides[1] > sides[0] && sides[1] > sides[2] )
axis = 1;
else
axis = 2;
REAL localTransform[16];
REAL maxDist = 0;
REAL maxLen = 0;
switch ( axis )
{
case 0:
{
fm_eulerMatrix(0,0,FM_PI/2,localTransform);
fm_matrixMultiply(localTransform,omatrix,matrix);
const unsigned char *scan = (const unsigned char *)points;
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *)scan;
REAL t[3];
fm_inverseRT(omatrix,p,t);
REAL dist = t[1]*t[1]+t[2]*t[2];
if ( dist > maxDist )
{
maxDist = dist;
}
REAL l = (REAL) fabs(t[0]);
if ( l > maxLen )
{
maxLen = l;
}
scan+=pstride;
}
}
height = sides[0];
break;
case 1:
{
fm_eulerMatrix(0,FM_PI/2,0,localTransform);
fm_matrixMultiply(localTransform,omatrix,matrix);
const unsigned char *scan = (const unsigned char *)points;
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *)scan;
REAL t[3];
fm_inverseRT(omatrix,p,t);
REAL dist = t[0]*t[0]+t[2]*t[2];
if ( dist > maxDist )
{
maxDist = dist;
}
REAL l = (REAL) fabs(t[1]);
if ( l > maxLen )
{
maxLen = l;
}
scan+=pstride;
}
}
height = sides[1];
break;
case 2:
{
fm_eulerMatrix(FM_PI/2,0,0,localTransform);
fm_matrixMultiply(localTransform,omatrix,matrix);
const unsigned char *scan = (const unsigned char *)points;
for (size_t i=0; i<vcount; i++)
{
const REAL *p = (const REAL *)scan;
REAL t[3];
fm_inverseRT(omatrix,p,t);
REAL dist = t[0]*t[0]+t[1]*t[1];
if ( dist > maxDist )
{
maxDist = dist;
}
REAL l = (REAL) fabs(t[2]);
if ( l > maxLen )
{
maxLen = l;
}
scan+=pstride;
}
}
height = sides[2];
break;
}
radius = (REAL)sqrt(maxDist);
height = (maxLen*2)-(radius*2);
}
inline REAL mb_sqr(REAL r) {return r*r;}
#ifndef BEST_FIT_SPHERE_H
#define BEST_FIT_SPHERE_H
namespace BEST_FIT_SPHERE
{
// Vec3 (inline)
// -------------
template <class Type> class Vec3
{
public:
// default
Vec3(void) {}
// copy from Vec3
Vec3 (const Vec3& p)
{
coord[0] = p.coord[0];
coord[1] = p.coord[1];
coord[2] = p.coord[2];
}
// copy from Type*
Vec3 (const Type* p)
{
coord[0] = p[0];
coord[1] = p[1];
coord[2] = p[2];
}
void set(const Type* p)
{
coord[0] = p[0];
coord[1] = p[1];
coord[2] = p[2];
}
// assignment
Vec3& operator = (const Vec3& p)
{
if (this != &p)
{
coord[0] = p.coord[0];
coord[1] = p.coord[1];
coord[2] = p.coord[2];
}
return *this;
}
// coordinate access
Type& operator [] (int i)
{
return coord[i];
}
const Type& operator [] (int i) const
{
return coord[i];
}
const Type* begin(void) const
{
return coord;
}
const Type* end(void) const
{
return coord+3;
}
private:
Type coord [3];
};
// SphereFit
// ----------
template <class Type> class SphereFit
{
public:
SphereFit() {reset();}
// access
const Type* center() const { return mCurrentCenter; }
Type squared_radius() const { return mCurrentSquaredRadius; }
int size() const { return mSize; }
int support_size() const { return mSupport; }
Type excess (const Vec3<Type>& p) const
{
Type e = -mCurrentSquaredRadius;
for (int k=0; k<3; ++k)
e += mb_sqr(p[k]-mCurrentCenter[k]);
return e;
}
// modification
void reset(void) // generates empty sphere with m=s=0
{
mSize = mSupport = 0;
// we misuse mC[0] for the center of the empty sphere
for (int j=0; j<3; ++j)
mC[0][j]=0;
mCurrentCenter = mC[0];
mCurrentSquaredRadius = -1;
}
bool push(const Vec3<Type>& p)
{
int i, j;
Type eps = 1e-15f;
if (mSize==0)
{
for (i=0; i<3; ++i)
mQ[i] = p[i];
for (i=0; i<3; ++i)
mC[0][i] = mQ[i];
mSquareRadius[0] = 0;
}
else
{
// set v_m to Q_m
for (i=0; i<3; ++i)
mV[mSize][i] = p[i]-mQ[i];
// compute the a_{m,i}, i< m
for (i=1; i<mSize; ++i)
{
mA[mSize][i] = 0;
for (j=0; j<3; ++j)
mA[mSize][i] += mV[i][j] * mV[mSize][j];
mA[mSize][i]*=(2/mZ[i]);
}
// update v_m to Q_m-\bar{Q}_m
for (i=1; i<mSize; ++i)
{
for (j=0; j<3; ++j)
{
mV[mSize][j] -= mA[mSize][i]*mV[i][j];
}
}
// compute z_m
mZ[mSize]=0;
for (j=0; j<3; ++j)
{
mZ[mSize] += mb_sqr(mV[mSize][j]);
}
mZ[mSize]*=2;
// reject push if z_m too small
if (mZ[mSize]<eps*mCurrentSquaredRadius)
{
return false;
}
// update c, mSquareRadius
Type e = -mSquareRadius[mSize-1];
for (i=0; i<3; ++i)
{
e += mb_sqr(p[i]-mC[mSize-1][i]);
}
mF[mSize]=e/mZ[mSize];
for (i=0; i<3; ++i)
{
mC[mSize][i] = mC[mSize-1][i]+mF[mSize]*mV[mSize][i];
}
mSquareRadius[mSize] = mSquareRadius[mSize-1] + e*mF[mSize]/2;
}
mCurrentCenter = mC[mSize];
mCurrentSquaredRadius = mSquareRadius[mSize];
mSupport = ++mSize;
return true;
}
void pop (void) { --mSize; };
// checking
Type slack(void) const
{
Type l[4], min_l=0;
l[0] = 1;
for (int i=mSupport-1; i>0; --i)
{
l[i] = mF[i];
for (int k=mSupport-1; k>i; --k)
{
l[i]-=mA[k][i]*l[k];
}
if (l[i] < min_l) min_l = l[i];
{
l[0] -= l[i];
}
}
if (l[0] < min_l)
min_l = l[0];
return ( (min_l < 0) ? -min_l : 0);
private:
// data members
int mSize;
int mSupport;
Type mQ[3];
Type mZ[4];
Type mF[4];
Type mV[4][3];
Type mA[4][3];
Type mC[4][3];
Type mSquareRadius[4];
Type* mCurrentCenter; // refers to some mC[j]
Type mCurrentSquaredRadius;
};
// --------
template <class Type> class BestFitSphere
{
public:
// builds the smallest enclosing sphere of the internal point set
Type computeBestFitSphere(unsigned int pcount,const Type *points,unsigned int
pstride,Type *center)
{
mPcount = pcount;
mPoints = new Vec3<Type>[pcount];
const unsigned char *scan = (const unsigned char *)points;
for (unsigned int i=0; i<pcount; i++)
{
Type *p = (Type *)scan;
mPoints[i].set(p);
scan+=pstride;
}
mBestFitSphere.reset();
mSupportEnd = 0;
pivot_mb(mPcount);
const Type *c = mBestFitSphere.center();
center[0] = c[0];
center[1] = c[1];
center[2] = c[2];
delete []mPoints;
mPoints = 0;
return (REAL)sqrt(mBestFitSphere.squared_radius());
}
private:
int nr_support_points (void) const
{
return mBestFitSphere.support_size();
}
Type accuracy (Type& slack) const
{
Type e, max_e = 0;
int n_supp=0;
unsigned int i;
for (i=0; i!=mSupportEnd; ++i,++n_supp)
if ((e = std::abs (mBestFitSphere.excess (mPoints[i]))) > max_e)
max_e = e;
// you've found a non-numerical problem if the following ever fails
assert (n_supp == nr_support_points());
for (i=mSupportEnd; i!=mPcount; ++i)
{
if ((e = mBestFitSphere.excess(mPoints[i])) > max_e)
{
max_e = e;
}
}
slack = mBestFitSphere.slack();
return (max_e/mBestFitSphere.squared_radius());
}
// returns true if the accuracy is below the given tolerance and the
// slack is 0
bool is_valid(Type tolerance = 1e-15f) const
{
Type slack;
return ( (accuracy (slack) < tolerance) && (slack == 0) );
}
void mtf_mb(unsigned int i)
{
mSupportEnd = 0;
if ((mBestFitSphere.size())==4) return;
for (unsigned int k=0; k!=i;)
{
unsigned int j= k++;
if (mBestFitSphere.excess(mPoints[j]) > 0)
{
if (mBestFitSphere.push(mPoints[j]))
{
mtf_mb(j);
mBestFitSphere.pop();
move_to_front(j);
}
}
}
}
void pivot_mb(unsigned int i)
{
unsigned int t = 1;
mtf_mb(t);
Type max_e, old_mSquareRadius = -1;
do
{
unsigned int pivot;
max_e = max_excess (t,i,pivot);
if (max_e > 0)
{
t = mSupportEnd;
if (t==pivot) ++t;
old_mSquareRadius = mBestFitSphere.squared_radius();
mBestFitSphere.push (mPoints[pivot]);
mtf_mb(mSupportEnd);
mBestFitSphere.pop();
move_to_front(pivot);
}
} while ((max_e > 0) && (mBestFitSphere.squared_radius() > old_mSquareRadius));
}
void move_to_front (unsigned int j)
{
if (mSupportEnd == j)
mSupportEnd++;
if ( j > 0 )
{
Vec3<Type> p = mPoints[j];
for (unsigned int i=j; i>0; i--)
{
mPoints[i] = mPoints[i-1];
}
mPoints[0] = p;
if ( mSupportEnd < j )
{
mSupportEnd++;
}
}
}
Type max_excess(unsigned int t,unsigned int i,unsigned int& pivot) const
{
const Type *c = mBestFitSphere.center(), mSquareRadius =
mBestFitSphere.squared_radius();
Type e, max_e = 0;
for (unsigned int k=t; k!=i; ++k)
{
const Type *p = (mPoints[k]).begin();
e = -mSquareRadius;
for (int j=0; j<3; ++j)
e += mb_sqr(p[j]-c[j]);
if (e > max_e)
{
max_e = e;
pivot = k;
}
}
return max_e;
}
// data members
unsigned int mPcount;
Vec3<Type> *mPoints;
SphereFit<Type> mBestFitSphere; // the current best fit sphere
unsigned int mSupportEnd; // past-the-end iterator of support set
};
}; // end of namespace
#endif
REAL fm_computeBestFitSphere(size_t vcount,const REAL *points,size_t pstride,REAL
*center)
{
BEST_FIT_SPHERE::BestFitSphere<REAL> mb;
return mb.computeBestFitSphere(vcount,points,pstride,center);
}