X-Git-Url: https://git.jsancho.org/?a=blobdiff_plain;f=Source%2FQuaternions.h;h=499bc0da0bd0fcc85151395b1b0f90d59d48a7ea;hb=cd043e3f9e26c2b3406b40a354c2840941e9db7f;hp=65bd8ae1d16fc16553224094620c7289ff3648be;hpb=8562aa5bebc97ea8a61d056aec3847030c6985b7;p=lugaru.git diff --git a/Source/Quaternions.h b/Source/Quaternions.h index 65bd8ae..499bc0d 100644 --- a/Source/Quaternions.h +++ b/Source/Quaternions.h @@ -1,12 +1,26 @@ +/* +Copyright (C) 2003, 2010 - Wolfire Games + +This file is part of Lugaru. + +Lugaru is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +Lugaru is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with Lugaru. If not, see . +*/ + #ifndef _QUATERNIONS_H_ #define _QUATERNIONS_H_ -#ifndef WIN32 -#pragma mark - -#endif - -//#include "Carbon.h" #include "math.h" #include "PhysicsMath.h" #include "gamegl.h" @@ -19,37 +33,36 @@ //using namespace std; typedef float Matrix_t [4][4]; -struct euler -{ - float x, y, z; +struct euler { + float x, y, z; }; -struct angle_axis -{ - float x, y, z, angle; +struct angle_axis { + float x, y, z, angle; }; -struct quaternion -{ - float x, y, z, w; +struct quaternion { + float x, y, z, w; }; -class XYZ{ +class XYZ +{ public: - float x; - float y; - float z; - inline XYZ operator+(XYZ add); - inline XYZ operator-(XYZ add); - inline XYZ operator*(float add); - inline XYZ operator*(XYZ add); - inline XYZ operator/(float add); - inline void operator+=(XYZ add); - inline void operator-=(XYZ add); - inline void operator*=(float add); - inline void operator*=(XYZ add); - inline void operator/=(float add); - inline void operator=(float add); - inline void vec(Vector add); - inline bool operator==(XYZ add); + float x; + float y; + float z; + XYZ() : x(0.0f), y(0.0f), z(0.0f) {} + inline XYZ operator+(XYZ add); + inline XYZ operator-(XYZ add); + inline XYZ operator*(float add); + inline XYZ operator*(XYZ add); + inline XYZ operator/(float add); + inline void operator+=(XYZ add); + inline void operator-=(XYZ add); + inline void operator*=(float add); + inline void operator*=(XYZ add); + inline void operator/=(float add); + inline void operator=(float add); + inline void vec(Vector add); + inline bool operator==(XYZ add); }; /*********************> Quaternion Function definition <********/ @@ -69,398 +82,435 @@ inline void Normalise(XYZ *vectory); inline float normaldotproduct(XYZ point1, XYZ point2); inline float fast_sqrt (register float arg); bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3); -bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p); -float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p); -float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ n, XYZ *p); -float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc,XYZ *n, XYZ *p); -float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc, XYZ *p); +bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p); +float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p); +float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ *p); +float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *n, XYZ *p); +float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *p); bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33); -bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p); -inline void ReflectVector(XYZ *vel, XYZ *n); +bool LineFacet(Vector p1, Vector p2, Vector pa, Vector pb, Vector pc, Vector *p); +inline void ReflectVector(XYZ *vel, const XYZ *n); +inline void ReflectVector(XYZ *vel, const XYZ &n); inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang); inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang); inline float findDistance(XYZ *point1, XYZ *point2); inline float findLength(XYZ *point1); inline float findLengthfast(XYZ *point1); -inline float findDistancefast(XYZ *point1, XYZ *point2); -inline float findDistancefast(XYZ point1, XYZ point2); -inline float findDistancefastflat(XYZ *point1, XYZ *point2); -inline float dotproduct(XYZ *point1, XYZ *point2); +inline float distsq(XYZ *point1, XYZ *point2); +inline float distsq(XYZ point1, XYZ point2); +inline float distsqflat(XYZ *point1, XYZ *point2); +inline float dotproduct(const XYZ *point1, const XYZ *point2); bool sphere_line_intersection ( - float x1, float y1 , float z1, - float x2, float y2 , float z2, - float x3, float y3 , float z3, float r ); + float x1, float y1 , float z1, + float x2, float y2 , float z2, + float x3, float y3 , float z3, float r ); bool sphere_line_intersection ( - XYZ *p1, XYZ *p2, XYZ *p3, float *r ); + XYZ *p1, XYZ *p2, XYZ *p3, float *r ); inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection ); -inline void Normalise(XYZ *vectory) { - static float d; - d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z); - if(d==0){return;} - vectory->x /= d; - vectory->y /= d; - vectory->z /= d; +inline void Normalise(XYZ *vectory) +{ + static float d; + d = fast_sqrt(vectory->x * vectory->x + vectory->y * vectory->y + vectory->z * vectory->z); + if (d == 0) { + return; + } + vectory->x /= d; + vectory->y /= d; + vectory->z /= d; } -inline XYZ XYZ::operator+(XYZ add){ - static XYZ ne; - ne=add; - ne.x+=x; - ne.y+=y; - ne.z+=z; - return ne; +inline XYZ XYZ::operator+(XYZ add) +{ + static XYZ ne; + ne = add; + ne.x += x; + ne.y += y; + ne.z += z; + return ne; } -inline XYZ XYZ::operator-(XYZ add){ - static XYZ ne; - ne=add; - ne.x=x-ne.x; - ne.y=y-ne.y; - ne.z=z-ne.z; - return ne; +inline XYZ XYZ::operator-(XYZ add) +{ + static XYZ ne; + ne = add; + ne.x = x - ne.x; + ne.y = y - ne.y; + ne.z = z - ne.z; + return ne; } -inline XYZ XYZ::operator*(float add){ - static XYZ ne; - ne.x=x*add; - ne.y=y*add; - ne.z=z*add; - return ne; +inline XYZ XYZ::operator*(float add) +{ + static XYZ ne; + ne.x = x * add; + ne.y = y * add; + ne.z = z * add; + return ne; } -inline XYZ XYZ::operator*(XYZ add){ - static XYZ ne; - ne.x=x*add.x; - ne.y=y*add.y; - ne.z=z*add.z; - return ne; +inline XYZ XYZ::operator*(XYZ add) +{ + static XYZ ne; + ne.x = x * add.x; + ne.y = y * add.y; + ne.z = z * add.z; + return ne; } -inline XYZ XYZ::operator/(float add){ - static XYZ ne; - ne.x=x/add; - ne.y=y/add; - ne.z=z/add; - return ne; +inline XYZ XYZ::operator/(float add) +{ + static XYZ ne; + ne.x = x / add; + ne.y = y / add; + ne.z = z / add; + return ne; } -inline void XYZ::operator+=(XYZ add){ - x+=add.x; - y+=add.y; - z+=add.z; +inline void XYZ::operator+=(XYZ add) +{ + x += add.x; + y += add.y; + z += add.z; } -inline void XYZ::operator-=(XYZ add){ - x=x-add.x; - y=y-add.y; - z=z-add.z; +inline void XYZ::operator-=(XYZ add) +{ + x = x - add.x; + y = y - add.y; + z = z - add.z; } -inline void XYZ::operator*=(float add){ - x=x*add; - y=y*add; - z=z*add; +inline void XYZ::operator*=(float add) +{ + x = x * add; + y = y * add; + z = z * add; } -inline void XYZ::operator*=(XYZ add){ - x=x*add.x; - y=y*add.y; - z=z*add.z; +inline void XYZ::operator*=(XYZ add) +{ + x = x * add.x; + y = y * add.y; + z = z * add.z; } -inline void XYZ::operator/=(float add){ - x=x/add; - y=y/add; - z=z/add; +inline void XYZ::operator/=(float add) +{ + x = x / add; + y = y / add; + z = z / add; } -inline void XYZ::operator=(float add){ - x=add; - y=add; - z=add; +inline void XYZ::operator=(float add) +{ + x = add; + y = add; + z = add; } -inline void XYZ::vec(Vector add){ - x=add.x; - y=add.y; - z=add.z; +inline void XYZ::vec(Vector add) +{ + x = add.x; + y = add.y; + z = add.z; } -inline bool XYZ::operator==(XYZ add){ - if(x==add.x&&y==add.y&&z==add.z)return 1; - return 0; +inline bool XYZ::operator==(XYZ add) +{ + if (x == add.x && y == add.y && z == add.z) + return 1; + return 0; } -inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V){ - V->x = P->y * Q->z - P->z * Q->y; - V->y = P->z * Q->x - P->x * Q->z; - V->z = P->x * Q->y - P->y * Q->x; +inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V) +{ + V->x = P->y * Q->z - P->z * Q->y; + V->y = P->z * Q->x - P->x * Q->z; + V->z = P->x * Q->y - P->y * Q->x; } -inline void CrossProduct(XYZ P, XYZ Q, XYZ *V){ - V->x = P.y * Q.z - P.z * Q.y; - V->y = P.z * Q.x - P.x * Q.z; - V->z = P.x * Q.y - P.y * Q.x; +inline void CrossProduct(XYZ P, XYZ Q, XYZ *V) +{ + V->x = P.y * Q.z - P.z * Q.y; + V->y = P.z * Q.x - P.x * Q.z; + V->z = P.x * Q.y - P.y * Q.x; } inline float fast_sqrt (register float arg) -{ +{ #if PLATFORM_MACOSX - // Can replace with slower return std::sqrt(arg); - register float result; + // Can replace with slower return std::sqrt(arg); + register float result; - if (arg == 0.0) return 0.0; + if (arg == 0.0) + return 0.0; - asm { - frsqrte result,arg // Calculate Square root - } + asm { + frsqrte result, arg // Calculate Square root + } - // Newton Rhapson iterations. - result = result + 0.5 * result * (1.0 - arg * result * result); - result = result + 0.5 * result * (1.0 - arg * result * result); + // Newton Rhapson iterations. + result = result + 0.5 * result * (1.0 - arg * result * result); + result = result + 0.5 * result * (1.0 - arg * result * result); - return result * arg; + return result * arg; #else - return sqrtf( arg); + return sqrtf( arg); #endif } -inline float normaldotproduct(XYZ point1, XYZ point2){ - static GLfloat returnvalue; - Normalise(&point1); - Normalise(&point2); - returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z); - return returnvalue; +inline float normaldotproduct(XYZ point1, XYZ point2) +{ + static GLfloat returnvalue; + Normalise(&point1); + Normalise(&point2); + returnvalue = (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z); + return returnvalue; } -inline void ReflectVector(XYZ *vel, XYZ *n) +inline void ReflectVector(XYZ *vel, const XYZ *n) { - static XYZ vn; - static XYZ vt; - static float dotprod; - - dotprod=dotproduct(n,vel); - vn.x=n->x*dotprod; - vn.y=n->y*dotprod; - vn.z=n->z*dotprod; - - vt.x=vel->x-vn.x; - vt.y=vel->y-vn.y; - vt.z=vel->z-vn.z; + ReflectVector(vel, *n); +} - vel->x = vt.x - vn.x; - vel->y = vt.y - vn.y; - vel->z = vt.z - vn.z; +inline void ReflectVector(XYZ *vel, const XYZ &n) +{ + static XYZ vn; + static XYZ vt; + static float dotprod; + + dotprod = dotproduct(&n, vel); + vn.x = n.x * dotprod; + vn.y = n.y * dotprod; + vn.z = n.z * dotprod; + + vt.x = vel->x - vn.x; + vt.y = vel->y - vn.y; + vt.z = vel->z - vn.z; + + vel->x = vt.x - vn.x; + vel->y = vt.y - vn.y; + vel->z = vt.z - vn.z; } -inline float dotproduct(XYZ *point1, XYZ *point2){ - static GLfloat returnvalue; - returnvalue=(point1->x*point2->x+point1->y*point2->y+point1->z*point2->z); - return returnvalue; +inline float dotproduct(const XYZ *point1, const XYZ *point2) +{ + static GLfloat returnvalue; + returnvalue = (point1->x * point2->x + point1->y * point2->y + point1->z * point2->z); + return returnvalue; } -inline float findDistance(XYZ *point1, XYZ *point2){ - return(fast_sqrt((point1->x-point2->x)*(point1->x-point2->x)+(point1->y-point2->y)*(point1->y-point2->y)+(point1->z-point2->z)*(point1->z-point2->z))); +inline float findDistance(XYZ *point1, XYZ *point2) +{ + return(fast_sqrt((point1->x - point2->x) * (point1->x - point2->x) + (point1->y - point2->y) * (point1->y - point2->y) + (point1->z - point2->z) * (point1->z - point2->z))); } -inline float findLength(XYZ *point1){ - return(fast_sqrt((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z))); +inline float findLength(XYZ *point1) +{ + return(fast_sqrt((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z))); } -inline float findLengthfast(XYZ *point1){ - return((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z)); +inline float findLengthfast(XYZ *point1) +{ + return((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z)); } -inline float findDistancefast(XYZ *point1, XYZ *point2){ - return((point1->x-point2->x)*(point1->x-point2->x)+(point1->y-point2->y)*(point1->y-point2->y)+(point1->z-point2->z)*(point1->z-point2->z)); +inline float distsq(XYZ *point1, XYZ *point2) +{ + return((point1->x - point2->x) * (point1->x - point2->x) + (point1->y - point2->y) * (point1->y - point2->y) + (point1->z - point2->z) * (point1->z - point2->z)); } -inline float findDistancefast(XYZ point1, XYZ point2){ - return((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z)); +inline float distsq(XYZ point1, XYZ point2) +{ + return((point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y) + (point1.z - point2.z) * (point1.z - point2.z)); } -inline float findDistancefastflat(XYZ *point1, XYZ *point2){ - return((point1->x-point2->x)*(point1->x-point2->x)+(point1->z-point2->z)*(point1->z-point2->z)); +inline float distsqflat(XYZ *point1, XYZ *point2) +{ + return((point1->x - point2->x) * (point1->x - point2->x) + (point1->z - point2->z) * (point1->z - point2->z)); } -inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){ - static XYZ newpoint; - if(xang){ - xang*=6.283185f; - xang/=360; - } - if(yang){ - yang*=6.283185f; - yang/=360; - } - if(zang){ - zang*=6.283185f; - zang/=360; - } - - - if(yang){ - newpoint.z=thePoint.z*cosf(yang)-thePoint.x*sinf(yang); - newpoint.x=thePoint.z*sinf(yang)+thePoint.x*cosf(yang); - thePoint.z=newpoint.z; - thePoint.x=newpoint.x; - } +inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang) +{ + static XYZ newpoint; + if (xang) { + xang *= 6.283185f; + xang /= 360; + } + if (yang) { + yang *= 6.283185f; + yang /= 360; + } + if (zang) { + zang *= 6.283185f; + zang /= 360; + } + + + if (yang) { + newpoint.z = thePoint.z * cosf(yang) - thePoint.x * sinf(yang); + newpoint.x = thePoint.z * sinf(yang) + thePoint.x * cosf(yang); + thePoint.z = newpoint.z; + thePoint.x = newpoint.x; + } + + if (zang) { + newpoint.x = thePoint.x * cosf(zang) - thePoint.y * sinf(zang); + newpoint.y = thePoint.y * cosf(zang) + thePoint.x * sinf(zang); + thePoint.x = newpoint.x; + thePoint.y = newpoint.y; + } + + if (xang) { + newpoint.y = thePoint.y * cosf(xang) - thePoint.z * sinf(xang); + newpoint.z = thePoint.y * sinf(xang) + thePoint.z * cosf(xang); + thePoint.z = newpoint.z; + thePoint.y = newpoint.y; + } + + return thePoint; +} - if(zang){ - newpoint.x=thePoint.x*cosf(zang)-thePoint.y*sinf(zang); - newpoint.y=thePoint.y*cosf(zang)+thePoint.x*sinf(zang); - thePoint.x=newpoint.x; - thePoint.y=newpoint.y; - } +inline float square( float f ) +{ + return (f * f) ; +} - if(xang){ - newpoint.y=thePoint.y*cosf(xang)-thePoint.z*sinf(xang); - newpoint.z=thePoint.y*sinf(xang)+thePoint.z*cosf(xang); - thePoint.z=newpoint.z; - thePoint.y=newpoint.y; - } +inline bool sphere_line_intersection ( + float x1, float y1 , float z1, + float x2, float y2 , float z2, + float x3, float y3 , float z3, float r ) +{ - return thePoint; + // x1,y1,z1 P1 coordinates (point of line) + // x2,y2,z2 P2 coordinates (point of line) + // x3,y3,z3, r P3 coordinates and radius (sphere) + // x,y,z intersection coordinates + // + // This function returns a pointer array which first index indicates + // the number of intersection point, followed by coordinate pairs. + + //~ static float x , y , z; + static float a, b, c, /*mu,*/ i ; + + if (x1 > x3 + r && x2 > x3 + r) return(0); + if (x1 < x3 - r && x2 < x3 - r) return(0); + if (y1 > y3 + r && y2 > y3 + r) return(0); + if (y1 < y3 - r && y2 < y3 - r) return(0); + if (z1 > z3 + r && z2 > z3 + r) return(0); + if (z1 < z3 - r && z2 < z3 - r) return(0); + a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1); + b = 2 * ( (x2 - x1) * (x1 - x3) + + (y2 - y1) * (y1 - y3) + + (z2 - z1) * (z1 - z3) ) ; + c = square(x3) + square(y3) + + square(z3) + square(x1) + + square(y1) + square(z1) - + 2 * ( x3 * x1 + y3 * y1 + z3 * z1 ) - square(r) ; + i = b * b - 4 * a * c ; + + if ( i < 0.0 ) { + // no intersection + return(0); + } + return(1); } -inline float square( float f ) { return (f*f) ;} - inline bool sphere_line_intersection ( - float x1, float y1 , float z1, - float x2, float y2 , float z2, - float x3, float y3 , float z3, float r ) -{ - - // x1,y1,z1 P1 coordinates (point of line) - // x2,y2,z2 P2 coordinates (point of line) - // x3,y3,z3, r P3 coordinates and radius (sphere) - // x,y,z intersection coordinates - // - // This function returns a pointer array which first index indicates - // the number of intersection point, followed by coordinate pairs. - - static float x , y , z; - static float a, b, c, mu, i ; - - if(x1>x3+r&&x2>x3+r)return(0); - if(x1y3+r&&y2>y3+r)return(0); - if(y1z3+r&&z2>z3+r)return(0); - if(z1y,p1->z P1 coordinates (point of line) + // p2->x,p2->y,p2->z P2 coordinates (point of line) + // p3->x,p3->y,p3->z, r P3 coordinates and radius (sphere) + // x,y,z intersection coordinates + // + // This function returns a pointer array which first index indicates + // the number of intersection point, followed by coordinate pairs. + + //~ static float x , y , z; + static float a, b, c, /*mu,*/ i ; + + if (p1->x > p3->x + *r && p2->x > p3->x + *r) return(0); + if (p1->x < p3->x - *r && p2->x < p3->x - *r) return(0); + if (p1->y > p3->y + *r && p2->y > p3->y + *r) return(0); + if (p1->y < p3->y - *r && p2->y < p3->y - *r) return(0); + if (p1->z > p3->z + *r && p2->z > p3->z + *r) return(0); + if (p1->z < p3->z - *r && p2->z < p3->z - *r) return(0); + a = square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z); + b = 2 * ( (p2->x - p1->x) * (p1->x - p3->x) + + (p2->y - p1->y) * (p1->y - p3->y) + + (p2->z - p1->z) * (p1->z - p3->z) ) ; + c = square(p3->x) + square(p3->y) + + square(p3->z) + square(p1->x) + + square(p1->y) + square(p1->z) - + 2 * ( p3->x * p1->x + p3->y * p1->y + p3->z * p1->z ) - square(*r) ; + i = b * b - 4 * a * c ; + + if ( i < 0.0 ) { + // no intersection + return(0); + } + return(1); } -inline bool sphere_line_intersection ( - XYZ *p1, XYZ *p2, XYZ *p3, float *r ) -{ - - // x1,p1->y,p1->z P1 coordinates (point of line) - // p2->x,p2->y,p2->z P2 coordinates (point of line) - // p3->x,p3->y,p3->z, r P3 coordinates and radius (sphere) - // x,y,z intersection coordinates - // - // This function returns a pointer array which first index indicates - // the number of intersection point, followed by coordinate pairs. - - static float x , y , z; - static float a, b, c, mu, i ; - - if(p1->x>p3->x+*r&&p2->x>p3->x+*r)return(0); - if(p1->xx-*r&&p2->xx-*r)return(0); - if(p1->y>p3->y+*r&&p2->y>p3->y+*r)return(0); - if(p1->yy-*r&&p2->yy-*r)return(0); - if(p1->z>p3->z+*r&&p2->z>p3->z+*r)return(0); - if(p1->zz-*r&&p2->zz-*r)return(0); - a = square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z); - b = 2* ( (p2->x - p1->x)*(p1->x - p3->x) - + (p2->y - p1->y)*(p1->y - p3->y) - + (p2->z - p1->z)*(p1->z - p3->z) ) ; - c = square(p3->x) + square(p3->y) + - square(p3->z) + square(p1->x) + - square(p1->y) + square(p1->z) - - 2* ( p3->x*p1->x + p3->y*p1->y + p3->z*p1->z ) - square(*r) ; - i = b * b - 4 * a * c ; - - if ( i < 0.0 ) - { - // no intersection - return(0); - } - return(1); -} - -inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){ - static XYZ newpoint; - static XYZ oldpoint; - - oldpoint=thePoint; - - if(yang!=0){ - newpoint.z=oldpoint.z*cosf(yang)-oldpoint.x*sinf(yang); - newpoint.x=oldpoint.z*sinf(yang)+oldpoint.x*cosf(yang); - oldpoint.z=newpoint.z; - oldpoint.x=newpoint.x; - } - - if(zang!=0){ - newpoint.x=oldpoint.x*cosf(zang)-oldpoint.y*sinf(zang); - newpoint.y=oldpoint.y*cosf(zang)+oldpoint.x*sinf(zang); - oldpoint.x=newpoint.x; - oldpoint.y=newpoint.y; - } - - if(xang!=0){ - newpoint.y=oldpoint.y*cosf(xang)-oldpoint.z*sinf(xang); - newpoint.z=oldpoint.y*sinf(xang)+oldpoint.z*cosf(xang); - oldpoint.z=newpoint.z; - oldpoint.y=newpoint.y; - } - - return oldpoint; +inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang) +{ + static XYZ newpoint; + static XYZ oldpoint; + + oldpoint = thePoint; + + if (yang != 0) { + newpoint.z = oldpoint.z * cosf(yang) - oldpoint.x * sinf(yang); + newpoint.x = oldpoint.z * sinf(yang) + oldpoint.x * cosf(yang); + oldpoint.z = newpoint.z; + oldpoint.x = newpoint.x; + } + + if (zang != 0) { + newpoint.x = oldpoint.x * cosf(zang) - oldpoint.y * sinf(zang); + newpoint.y = oldpoint.y * cosf(zang) + oldpoint.x * sinf(zang); + oldpoint.x = newpoint.x; + oldpoint.y = newpoint.y; + } + + if (xang != 0) { + newpoint.y = oldpoint.y * cosf(xang) - oldpoint.z * sinf(xang); + newpoint.z = oldpoint.y * sinf(xang) + oldpoint.z * cosf(xang); + oldpoint.z = newpoint.z; + oldpoint.y = newpoint.y; + } + + return oldpoint; } inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection ) { - float LineMag; - float U; + float LineMag; + float U; - LineMag = findDistance( LineEnd, LineStart ); + LineMag = findDistance( LineEnd, LineStart ); - U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) + - ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) + - ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) / - ( LineMag * LineMag ); + U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) + + ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) + + ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) / + ( LineMag * LineMag ); - if( U < 0.0f || U > 1.0f ) - return 0; // closest point does not fall within the line segment + if ( U < 0.0f || U > 1.0f ) + return 0; // closest point does not fall within the line segment - Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x ); - Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y ); - Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z ); + Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x ); + Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y ); + Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z ); - *Distance = findDistance( Point, Intersection ); + *Distance = findDistance( Point, Intersection ); - return 1; + return 1; } -#endif \ No newline at end of file +#endif