123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273 |
- /*!
- *
- * \file src/MatMath.cpp
- * \brief Vector and Matrix math
- *
- * \author Mongoose
- */
-
- #include <stdlib.h>
- #include <math.h>
- #include <float.h>
-
- #include "MatMath.h"
- #include "Vector3d.h"
- #include "Matrix.h"
-
- bool equalEpsilon(vec_t a, vec_t b) {
- vec_t epsilon = FLT_EPSILON;
- if (fabs(a - b) <= (((fabs(b) > fabs(a)) ? fabs(b) : fabs(a)) * epsilon))
- return true;
- return false;
- }
-
- vec_t helIntersectionOfAbstractSpheres(vec3_t centerA, vec_t radiusA,
- vec3_t centerB, vec_t radiusB)
- {
- Vector3d a = Vector3d(centerA);
- Vector3d b = Vector3d(centerB);
- Vector3d d = a - b;
- vec_t dist, minDist;
-
- dist = Vector3d::dot(d, d);
- minDist = radiusA + radiusB;
-
- return (dist <= minDist * minDist);
- }
-
-
- inline vec_t square(vec_t a)
- {
- return a * a;
- }
-
-
- // Returns number of intersections and intersection position(s)
- // Got algorithm from http://astronomy.swin.edu.au/~pbourke/geometry/
- int helIntersectionOfAbstractSphereAndLine(vec3_t center, vec_t radius,
- vec3_t posA, vec3_t posB,
- vec3_t intersectionA,
- vec3_t intersectionB)
- {
- // float x , y , z;
- vec_t a, b, c, mu, i ;
-
-
- a = (square(posB[0] - posA[0]) +
- square(posB[1] - posA[1]) +
- square(posB[2] - posA[2]));
- b = (2 * ((posB[0] - posA[0]) * (posA[0] - center[0]) +
- (posB[1] - posA[1]) * (posA[1] - center[1]) +
- (posB[2] - posA[2]) * (posA[2] - center[2])));
- c = (square(center[0]) + square(center[1]) +
- square(center[2]) + square(posA[0]) +
- square(posA[1]) + square(posA[2]) -
- 2 * (center[0]*posA[0] + center[1]*posA[1] + center[2]*posA[2]) -
- square(radius));
-
- i = b * b - 4 * a * c;
-
-
- if (i < 0.0)
- {
- // No intersection
- return 0;
- }
- else if (i == 0.0)
- {
- // One intersection
- mu = -b/(2*a) ;
- intersectionA[0] = posA[0] + mu*(posB[0]-posA[0]);
- intersectionA[1] = posA[1] + mu*(posB[1]-posA[1]);
- intersectionA[2] = posA[2] + mu*(posB[2]-posA[2]);
-
- return 1;
- }
- else
- {
- // Two intersections
-
- // First intersection
- mu = (-b + sqrtf( square(b) - 4.0f*a*c)) / (2.0f*a);
- intersectionA[0] = posA[0] + mu*(posB[0]-posA[0]);
- intersectionA[1] = posA[1] + mu*(posB[1]-posA[1]);
- intersectionA[2] = posA[2] + mu*(posB[2]-posA[2]);
-
- // Second intersection
- mu = (-b - sqrtf(square(b) - 4.0f*a*c)) / (2.0f*a);
- intersectionB[0] = posA[0] + mu*(posB[0]-posA[0]);
- intersectionB[1] = posA[1] + mu*(posB[1]-posA[1]);
- intersectionB[2] = posA[2] + mu*(posB[2]-posA[2]);
-
- return 2;
- }
- }
-
-
- int helIntersectionLineAndPolygon(vec3_t intersect,
- vec3_t p1, vec3_t p2,
- vec3_t *polygon)
- {
- // vec3_t normal, a, b;
- Vector3d a, b, normal, pA, pB;
- vec_t d, denominator, mu;
-
-
- pA = Vector3d(p1);
- pB = Vector3d(p2);
-
- // Find normal
- //mtkVectorSubtract(polygon[1], polygon[0], a);
- a = Vector3d(polygon[1]) - Vector3d(polygon[0]);
- //mtkVectorSubtract(polygon[2], polygon[0], b);
- b = Vector3d(polygon[2]) - Vector3d(polygon[0]);
- normal = Vector3d::cross(a, b);
- //mtkVectorCrossProduct(a, b, normal);
- normal.normalize();
- //mtkVectorNormalize(normal, normal);
-
- // find D
- //d = (normal[0] * polygon[0][0] -
- // normal[1] * polygon[0][1] -
- // normal[2] * polygon[0][2]);
- d = (normal.mVec[0] * polygon[0][0] -
- normal.mVec[1] * polygon[0][1] -
- normal.mVec[2] * polygon[0][2]);
-
- // line segment parallel to plane?
- //mtkVectorSubtract(p2, p1, a); // cache p2 - p1 => a
- a = pB - pA;
-
- //denominator = (normal[0] * a[0] +
- // normal[1] * a[1] +
- // normal[2] * a[2]);
- denominator = Vector3d::dot(normal, a);
-
- if (denominator > 0.0)
- return 0;
-
- // Line segment contains intercept point?
- //mu = - ((d + normal[0] * p1[0] + normal[1] * p1[1] + normal[2] * p1[2]) /
- // denominator);
- mu = -((d + Vector3d::dot(normal, pA)) / denominator);
-
- if (mu < 0.0 || mu > 1.0)
- return 0;
-
- //intersect[0] = p1[0] + mu * a[0];
- //intersect[1] = p1[1] + mu * a[1];
- //intersect[2] = p1[2] + mu * a[2];
- b = pA + (a * mu);
- intersect[0] = b.mVec[0];
- intersect[1] = b.mVec[1];
- intersect[2] = b.mVec[2];
-
-
- // See if the intercept is bound by polygon by winding number
- #ifdef WINDING_NUMBERS_TRIANGLE
- mtkVectorSubtract(polygon[0], intersect, a);
- mtkVectorNormalize(a, a);
- mtkVectorSubtract(polygon[1], intersect, b);
- mtkVectorNormalize(b, b);
- mtkVectorSubtract(polygon[2], intersect, c);
- mtkVectorNormalize(c, c);
-
- t0 = mtkVectorDotProduct(a, b);
- t1 = mtkVectorDotProduct(b, c);
- t2 = mtkVectorDotProduct(c, a);
-
- total = HEL_RAD_TO_DEG(acos(t0) + acos(t1) + acos(t2));
-
- if (total - 360 < 0.0)
- return 0;
- #else // assume convex polygons here for sure
- //mtkVectorSubtract(intersect, polygon[0], a);
- //theta = mtkVectorDotProduct(a, normal);
- double theta = Vector3d::dot(b - Vector3d(polygon[0]), normal); // b = intersect
-
- if (theta >= 90.0) // Yeah I know
- return 0;
- #endif
-
- return 1;
- }
-
-
- vec_t helDistToSphereFromPlane3v(vec3_t center, vec_t radius, vec4_t plane)
- {
- vec_t d;
-
-
- d = (plane[0] * center[0] +
- plane[1] * center[1] +
- plane[2] * center[2] +
- plane[3]);
-
- if (d <= -radius)
- return 0;
-
- return d + radius;
- }
-
-
- vec_t helDistToBboxFromPlane3v(vec3_t min, vec3_t max, vec4_t plane)
- {
- vec3_t center;
- vec_t d, radius;
-
-
- helMidpoint3v(min, max, center);
-
- d = (plane[0] * center[0] +
- plane[1] * center[1] +
- plane[2] * center[2] +
- plane[3]);
-
- radius = helDist3v(max, center);
-
- if (d <= -radius)
- return 0;
-
- return d + radius;
- }
-
-
- vec_t helDist3v(vec3_t a, vec3_t b)
- {
- return (sqrtf( ((b[0] - a[0]) * (b[0] - a[0])) +
- ((b[1] - a[1]) * (b[1] - a[1])) +
- ((b[2] - a[2]) * (b[2] - a[2]))));
- }
-
-
- void helMidpoint3v(vec3_t a, vec3_t b, vec3_t mid)
- {
- mid[0] = (a[0] + b[0]) / 2.0f;
- mid[1] = (a[1] + b[1]) / 2.0f;
- mid[2] = (a[2] + b[2]) / 2.0f;
- }
-
-
- vec_t helNorm4v(vec4_t v)
- {
- return (sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]));
- }
-
-
- vec_t helNorm3v(vec3_t v)
- {
- return (sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
- }
-
-
- vec_t helNorm2v(vec2_t v)
- {
- return (sqrtf(v[0]*v[0] + v[1]*v[1]));
- }
-
-
- vec_t helRandomNum(vec_t from, vec_t to)
- {
- return from + ((to - from) * rand() / (RAND_MAX + 1.0f));
- }
|