// 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 // #ifdef _MSC_VER #pragma warning(disable:4996) #endif namespace FLOAT_MATH { void fm_inverseRT(const REAL matrix[16],const REAL pos[3],REAL t[3]) // inverse rotate translate the point. { … } REAL fm_getDeterminant(const REAL matrix[16]) { … } 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]) { … } void fm_getSubMatrix(int32_t ki,int32_t kj,REAL pDst[16],const REAL matrix[16]) { … } void fm_inverseTransform(const REAL matrix[16],REAL inverse_matrix[16]) { … } void fm_identity(REAL matrix[16]) // set 4x4 matrix to identity. { … } void fm_quatToEuler(const REAL quat[4],REAL &ax,REAL &ay,REAL &az) { … } void fm_eulerToMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero) { … } void fm_getAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) { … } void fm_eulerToQuat(const REAL *euler,REAL *quat) // convert euler angles to quaternion. { … } void fm_eulerToQuat(REAL roll,REAL pitch,REAL yaw,REAL *quat) // convert euler angles to quaternion. { … } void fm_quatToMatrix(const REAL *quat,REAL *matrix) // convert quaterinion rotation to matrix, zeros out the translation component. { … } void fm_quatRotate(const REAL *quat,const REAL *v,REAL *r) // rotate a vector directly by a quaternion. { … } void fm_getTranslation(const REAL *matrix,REAL *t) { … } 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 fm_sphereVolume(REAL radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed ) { … } REAL fm_cylinderVolume(REAL radius,REAL h) { … } REAL fm_capsuleVolume(REAL radius,REAL h) { … } void fm_transform(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and translate this point { … } void fm_rotate(const REAL matrix[16],const REAL v[3],REAL t[3]) // rotate and translate this point { … } REAL fm_distance(const REAL *p1,const REAL *p2) { … } REAL fm_distanceSquared(const REAL *p1,const REAL *p2) { … } REAL fm_distanceSquaredXZ(const REAL *p1,const REAL *p2) { … } REAL fm_computePlane(const REAL *A,const REAL *B,const REAL *C,REAL *n) // returns D { … } REAL fm_distToPlane(const REAL *plane,const REAL *p) // computes the distance of this point from the plane. { … } REAL fm_dot(const REAL *p1,const REAL *p2) { … } void fm_cross(REAL *cross,const REAL *a,const REAL *b) { … } REAL fm_computeNormalVector(REAL *n,const REAL *p1,const REAL *p2) { … } bool fm_computeWindingOrder(const REAL *p1,const REAL *p2,const REAL *p3) // returns true if the triangle is clockwise. { … } REAL fm_normalize(REAL *n) // normalize this vector { … } void fm_matrixMultiply(const REAL *pA,const REAL *pB,REAL *pM) { … } void fm_eulerToQuatDX(REAL x,REAL y,REAL z,REAL *quat) // convert euler angles to quaternion using the fucked up DirectX method { … } // 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. { … } void fm_scale(REAL x,REAL y,REAL z,REAL *fscale) // apply scale to the matrix. { … } void fm_composeTransform(const REAL *position,const REAL *quat,const REAL *scale,REAL *matrix) { … } void fm_setTranslation(const REAL *translation,REAL *matrix) { … } 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). */ { … } 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 fm_computeArea(const REAL *p1,const REAL *p2,const REAL *p3) { … } void fm_lerp(const REAL *p1,const REAL *p2,REAL *dest,REAL lerpValue) { … } 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 fm_insideAABB(const REAL *pos,const REAL *bmin,const REAL *bmax) { … } uint32_t fm_clipTestPoint(const REAL *bmin,const REAL *bmax,const REAL *pos) { … } uint32_t fm_clipTestPointXZ(const REAL *bmin,const REAL *bmax,const REAL *pos) // only tests X and Z, not Y { … } uint32_t fm_clipTestAABB(const REAL *bmin,const REAL *bmax,const REAL *p1,const REAL *p2,const REAL *p3,uint32_t &andCode) { … } bool intersect(const REAL *si,const REAL *ei,const REAL *bmin,const REAL *bmax,REAL *time) { … } bool fm_lineTestAABB(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time) { … } bool fm_lineTestAABBXZ(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time) { … } void fm_minmax(const REAL *p,REAL *bmin,REAL *bmax) // accmulate to a min-max value { … } REAL fm_solveX(const REAL *plane,REAL y,REAL z) // solve for X given this plane equation and the other two components. { … } REAL fm_solveY(const REAL *plane,REAL x,REAL z) // solve for Y given this plane equation and the other two components. { … } REAL fm_solveZ(const REAL *plane,REAL x,REAL y) // solve for Y given this plane equation and the other two components. { … } void fm_getAABBCenter(const REAL *bmin,const REAL *bmax,REAL *center) { … } FM_Axis fm_getDominantAxis(const REAL normal[3]) { … } bool fm_lineSphereIntersect(const REAL *center,REAL radius,const REAL *p1,const REAL *p2,REAL *intersect) { … } #define DOT(p1,p2) … bool fm_raySphereIntersect(const REAL *center,REAL radius,const REAL *pos,const REAL *dir,REAL distance,REAL *intersect) { … } void fm_catmullRom(REAL *out_vector,const REAL *p1,const REAL *p2,const REAL *p3,const REAL *p4, const REAL s) { … } bool fm_intersectAABB(const REAL *bmin1,const REAL *bmax1,const REAL *bmin2,const REAL *bmax2) { … } 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 { … } // 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 fm_distancePointLineSegment(const REAL *Point,const REAL *LineStart,const REAL *LineEnd,REAL *intersection,LineSegmentType &type,REAL epsilon) { … } #ifndef BEST_FIT_PLANE_H #define BEST_FIT_PLANE_H template <class Type> class Eigen { … }; #endif bool fm_computeBestFitPlane(uint32_t vcount, const REAL *points, uint32_t vstride, const REAL *weights, uint32_t wstride, REAL *plane, REAL *center) { … } bool fm_colinear(const REAL a1[3],const REAL a2[3],const REAL b1[3],const REAL b2[3],REAL epsilon) // true if these two line segments are co-linear. { … } bool fm_colinear(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon) { … } void fm_initMinMax(const REAL *p,REAL *bmin,REAL *bmax) { … } IntersectResult fm_intersectLineSegments2d(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL *intersection) { … } IntersectResult fm_intersectLineSegments2dTime(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL &t1,REAL &t2) { … } //**** Plane Triangle Intersection // assumes that the points are on opposite sides of the plane! bool fm_intersectPointPlane(const REAL *p1,const REAL *p2,REAL *split,const REAL *plane) { … } PlaneTriResult fm_getSidePlane(const REAL *p,const REAL *plane,REAL epsilon) { … } #ifndef PLANE_TRIANGLE_INTERSECTION_H #define PLANE_TRIANGLE_INTERSECTION_H #define MAXPTS … template <class Type> class point { … }; template <class Type> class plane { … }; template <class Type> class polygon { … }; #endif static inline void add(const REAL *p,REAL *dest,uint32_t tstride,uint32_t &pcount) { … } PlaneTriResult fm_planeTriIntersection(const REAL *_plane, // the plane equation in Ax+By+Cz+D format const REAL *triangle, // the source triangle. uint32_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 uint32_t &fcount, // number of vertices in the 'front' triangle REAL *back, // the triangle in back of the plane uint32_t &bcount) // the number of vertices in the 'back' triangle. { … } // computes the OBB for this set of points relative to this transform matrix. void computeOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix) { … } void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix,bool bruteForce) { … } void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos,REAL *quat,bool bruteForce) { … } void fm_computeBestFitABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos) { … } void fm_planeToMatrix(const REAL *plane,REAL *matrix) // convert a plane equation to a 4x4 rotation matrix { … } void fm_planeToQuat(const REAL *plane,REAL *quat,REAL *pos) // convert a plane equation to a quaternion and translation { … } void fm_eulerMatrix(REAL ax,REAL ay,REAL az,REAL *matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero) { … } //********************************************************** //********************************************************** //**** Vertex Welding //********************************************************** //********************************************************** #ifndef VERTEX_INDEX_H #define VERTEX_INDEX_H namespace VERTEX_INDEX { class KdTreeNode; KdTreeNodeVector; enum Axes { … }; class KdTreeFindNode { … }; class KdTreeInterface { … }; class KdTreeNode { … }; #define MAX_BUNDLE_SIZE … class KdTreeNodeBundle { … }; DoubleVector; FloatVector; class KdTree : public KdTreeInterface { … }; }; // end of namespace VERTEX_INDEX class MyVertexIndex : public fm_VertexIndex { … }; 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) { … } #endif // END OF VERTEX WELDING CODE REAL fm_computeBestFitAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) // returns the diagonal distance { … } /* a = b - c */ #define vector(a,b,c) … #define innerProduct(v,q) … #define crossProduct(a,b,c) … bool fm_lineIntersectsTriangle(const REAL *rayStart,const REAL *rayEnd,const REAL *p1,const REAL *p2,const REAL *p3,REAL *sect) { … } bool fm_rayIntersectsTriangle(const REAL *p,const REAL *d,const REAL *v0,const REAL *v1,const REAL *v2,REAL &t) { … } inline REAL det(const REAL *p1,const REAL *p2,const REAL *p3) { … } REAL fm_computeMeshVolume(const REAL *vertices,uint32_t tcount,const uint32_t *indices) { … } const REAL * fm_getPoint(const REAL *points,uint32_t pstride,uint32_t index) { … } bool fm_insideTriangle(REAL Ax, REAL Ay, REAL Bx, REAL By, REAL Cx, REAL Cy, REAL Px, REAL Py) { … } REAL fm_areaPolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride) { … } bool fm_pointInsidePolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride,const REAL *point,uint32_t xindex,uint32_t yindex) { … } uint32_t fm_consolidatePolygon(uint32_t pcount,const REAL *points,uint32_t pstride,REAL *_dest,REAL epsilon) // collapses co-linear edges. { … } #ifndef RECT3D_TEMPLATE #define RECT3D_TEMPLATE template <class T> class Rect3d { … }; #endif void splitRect(uint32_t axis, const Rect3d<REAL> &source, Rect3d<REAL> &b1, Rect3d<REAL> &b2, const REAL *midpoint) { … } bool fm_computeSplitPlane(uint32_t vcount, const REAL *vertices, uint32_t /* tcount */, const uint32_t * /* indices */, REAL *plane) { … } #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) { … } REAL fm_areaTriangle(const REAL *p0,const REAL *p1,const REAL *p2) { … } void fm_subtract(const REAL *A,const REAL *B,REAL *diff) // compute A-B and store the result in 'diff' { … } void fm_multiplyTransform(const REAL *pA,const REAL *pB,REAL *pM) { … } void fm_multiply(REAL *A,REAL scaler) { … } void fm_add(const REAL *A,const REAL *B,REAL *sum) { … } void fm_copy3(const REAL *source,REAL *dest) { … } uint32_t fm_copyUniqueVertices(uint32_t vcount,const REAL *input_vertices,REAL *output_vertices,uint32_t tcount,const uint32_t *input_indices,uint32_t *output_indices) { … } bool fm_isMeshCoplanar(uint32_t tcount,const uint32_t *indices,const REAL *vertices,bool doubleSided) // returns true if this collection of indexed triangles are co-planar! { … } bool fm_samePlane(const REAL p1[4],const REAL p2[4],REAL normalEpsilon,REAL dEpsilon,bool doubleSided) { … } void fm_initMinMax(REAL bmin[3],REAL bmax[3]) { … } void fm_inflateMinMax(REAL bmin[3], REAL bmax[3], REAL ratio) { … } #ifndef TESSELATE_H #define TESSELATE_H UintVector; class Myfm_Tesselate : public fm_Tesselate { … }; fm_Tesselate * fm_createTesselate(void) { … } void fm_releaseTesselate(fm_Tesselate *t) { … } #endif #ifndef RAY_ABB_INTERSECT #define RAY_ABB_INTERSECT //! Integer representation of a floating-point value. #define IR(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 … bool fm_intersectRayAABB(const float MinB[3],const float MaxB[3],const float origin[3],const float dir[3],float coord[3]) { … } bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const float p1[3],const float p2[3],float intersect[3]) { … } #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]) { … } const REAL * computePos(uint32_t index,const REAL *vertices,uint32_t vstride) { … } void computeNormal(uint32_t index,REAL *normals,uint32_t nstride,const REAL *normal) { … } void fm_computeMeanNormals(uint32_t vcount, // the number of vertices const REAL *vertices, // the base address of the vertex position data. uint32_t vstride, // the stride between position data. REAL *normals, // the base address of the destination for mean vector normals uint32_t nstride, // the stride between normals uint32_t tcount, // the number of triangles const uint32_t *indices) // the triangle indices { … } #endif #define BIGNUMBER … static inline void Set(REAL *n,REAL x,REAL y,REAL z) { n[0] = x; n[1] = y; n[2] = z; }; static inline void Copy(REAL *dest,const REAL *source) { … } REAL fm_computeBestFitSphere(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *center) { … } void fm_computeBestFitCapsule(uint32_t vcount,const REAL *points,uint32_t pstride,REAL &radius,REAL &height,REAL matrix[16],bool bruteForce) { … } //************* Triangulation #ifndef TRIANGULATE_H #define TRIANGULATE_H TU32; class TVec { … }; TVecVector; TU32Vector; class CTriangulator { … }; /// Default constructor CTriangulator::CTriangulator(void) { … } /// Default destructor CTriangulator::~CTriangulator() { … } /// Triangulates the contour void CTriangulator::triangulate(TU32Vector &indices) { … } /// Processes the triangulation void CTriangulator::_process(TU32Vector &indices) { … } /// Returns the area of the contour double CTriangulator::_area() { … } bool CTriangulator::_snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V) { … } /// Tests if a point is inside the given triangle bool CTriangulator::_insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P) { … } class Triangulate : public fm_Triangulate { … }; fm_Triangulate * fm_createTriangulate(void) { … } void fm_releaseTriangulate(fm_Triangulate *t) { … } #endif bool validDistance(const REAL *p1,const REAL *p2,REAL epsilon) { … } bool fm_isValidTriangle(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon) { … } void fm_multiplyQuat(const REAL *left,const REAL *right,REAL *quat) { … } bool fm_computeCentroid(uint32_t vcount, // number of input data points const REAL *points, // starting address of points array. REAL *center) { … } bool fm_computeCentroid(uint32_t vcount, // number of input data points const REAL *points, // starting address of points array. uint32_t triCount, const uint32_t *indices, REAL *center) { … } #ifndef TEMPLATE_VEC3 #define TEMPLATE_VEC3 template <class Type> class Vec3 { … }; #endif void fm_transformAABB(const REAL bmin[3],const REAL bmax[3],const REAL matrix[16],REAL tbmin[3],REAL tbmax[3]) { … } REAL fm_normalizeQuat(REAL n[4]) // normalize this quat { … } }; // end of namespace
Codebase Browser
godot