/* ** Copyright (C) 1996, 1997 Microsoft Corporation. All Rights Reserved. ** ** File: HitTest.cpp ** ** Author: ** ** Description: ** ** History: */ #include "pch.h" void HitTest::SetBB(Time start, Time stop, float dt) { assert (stop >= start); assert (m_timeStop >= m_timeStart); if (m_staticF) { m_tStart = 0.0f; m_tStop = dt; m_lastTest = NULL; } else { if (m_timeStart == m_timeStop) { //hittest always exists (or, at least, started before //start and ends after stop) m_tStart = 0.0f; m_tStop = dt; m_stopPosition = GetPosition() + m_velocity * m_tStop; } else if (stop < m_timeStart) { //The object shouldn't exist yet ... flag it as dead m_deadF = true; m_stopPosition = GetPosition(); } else if (m_timeStop > start) { if (start < m_timeStart) { //The object does not come into existance until after start. m_tStart = m_timeStart - start; //Back the object up so that it is in the correct position at time 0. SetPosition(GetPosition() - m_velocity * m_tStart); m_deadF = false; //just in case it was disabled by the above test } else { m_tStart = 0.0f; assert (!m_deadF); } m_tStop = ((m_timeStop < stop) ? m_timeStop : stop) - start; m_stopPosition = GetPosition() + m_velocity * m_tStop; } else { //The will die before the time interval of interest starts m_deadF = true; m_stopPosition = GetPosition(); } assert (m_stopPosition.LengthSquared() >= 0.0f); assert (m_stopPosition.LengthSquared() < 1.0e12f); UpdateBB(); } } void HitTest::UpdateBB(void) { if (!m_staticF) { Vector startPosition = m_tStart == 0.0f ? GetPosition() : GetPosition() + m_velocity * m_tStart; //offset for delayed start //let's assume were using the xy & z axes. assert (c_nAxes == 3); for (int i = 0; (i < c_nAxes); i++) { float v1 = startPosition[i]; float v2 = m_stopPosition[i]; if (v1 <= v2) { m_boundingBox.axes[i].values[0] = v1 - m_radius; m_boundingBox.axes[i].values[1] = v2 + m_radius; } else { m_boundingBox.axes[i].values[0] = v2 - m_radius; m_boundingBox.axes[i].values[1] = v1 + m_radius; } m_endpoints[i][0].value = m_boundingBox.axes[i].values[0]; m_endpoints[i][1].value = m_boundingBox.axes[i].values[1]; } } m_lastTest = NULL; } const double epsilon = 1.0f / 128.0f; void HitTest::Collide(HitTest* pHitTest, CollisionQueue* pQueue) { assert (pQueue); const BoundingBox& hitTestBB = pHitTest->m_boundingBox; if ((m_boundingBox.axes[0].values[1] > hitTestBB.axes[0].values[0]) && (m_boundingBox.axes[0].values[0] < hitTestBB.axes[0].values[1]) && (m_boundingBox.axes[1].values[1] > hitTestBB.axes[1].values[0]) && (m_boundingBox.axes[1].values[0] < hitTestBB.axes[1].values[1]) && (m_boundingBox.axes[2].values[1] > hitTestBB.axes[2].values[0]) && (m_boundingBox.axes[2].values[0] < hitTestBB.axes[2].values[1])) { //Calculate the time when pHitTest and this first collide (if ever) Vector dP = GetPosition() - pHitTest->GetPosition(); float r = pHitTest->m_radius + m_radius; double c = (double)(dP * dP) - (double)(r * r); Vector dV = pHitTest->m_velocity - m_velocity; //negative to eliminate an extraneous - below //Distance(t) = |dP - dV * t| //Distance(t)^2 = (dP - dV * t) * (dP - dV * t) // = (dP * dP - 2 * dP * dV * t + dV * dV * t^2) //Solve for Distance(t)^2 == radius^2 : a*t^2 - b * t + c = 0 => t = (b +- sqrt(b^2 - 4ac)) / 2a // bool fCollision = false; float tCollision; float tMax; double halfB = (dP * dV); //b/2 if (halfB > 0.0) //objects need to be closing at least a little to generate a collision { double a = dV * dV; if (a > 0.0) //Shouldn't be a problem but ... float do have their limits of resolution { double b2ac = halfB*halfB - a * c; if (b2ac >= 0.0) { //real roots ... at some point (past or future), they'll be close enough to collide //pick the minimum time: when they would first collide double s = sqrt(b2ac); tCollision = (c <= 0.0) ? 0.0f //Bounding spheres already intersect : (float)((halfB - s) / a); tMax = (float)((halfB + s) / a); if (tMax > m_tStop) tMax = m_tStop; fCollision = true; } } } else if (c < 0.0) { //The object bounding sphere's overlap ... we have a possible collision even if the //objects are not moving (or are moving apart) tCollision = m_tStart; tMax = FLT_MAX; if (halfB != 0.0) { //The objects are moving apart ... when will their bounding spheres no longer overlap. double a = dV * dV; if (a > 0.0) //Shouldn't be a problem but ... float do have their limits of resolution { tMax = (float)((halfB + (float)sqrt(halfB*halfB - a * c)) / a); } } if (tMax > m_tStop) tMax = m_tStop; fCollision = true; } //Is it in the window of time we are interested in? //The range of legal times is always updated in updateBB() and //projectiles will always have the most restrictive range (and //in a projectile vs. non-projectile, the projectile is always this). // //Allow collisions that occur slightly beyond m_tStop to guarantee that //all collisions get processed at least once (this could lead to collisions //happening twice but the less of a problem than missing a collision) if (fCollision && (tCollision >= m_tStart) && (tCollision <= m_tStop + (float)epsilon)) { HitTestShape htsThis = m_shape; HitTestShape htsPHT = pHitTest->m_shape; if (((m_shape <= c_htsSphere) && (pHitTest->m_shape <= c_htsSphere)) || HullCollide(&tCollision, tMax + (float)epsilon, this, &htsThis, pHitTest, &htsPHT, dP, dV)) { pQueue->addCollision(tCollision, this, htsThis, pHitTest, htsPHT); } } } } class BoundingPoint : public HitTest { public: BoundingPoint(IObject* d, bool staticF) : HitTest(d, 0.0f, staticF, c_htsPoint) { } }; class BoundingSphere : public HitTest { public: BoundingSphere(IObject* d, bool staticF) : HitTest(d, 0.0f, staticF, c_htsSphere) { } }; class ConvexVertex { public: const Vector& GetPosition(void) const { return m_position; } void SetPosition(const Vector& p) { m_position = p; } const ConvexVertex** GetAdjacencies(void) const { return m_adjacencies; } private: Vector m_position; const ConvexVertex** m_adjacencies; friend class HitTest; friend class BoundingHull; friend class SingleHull; }; class BoundingCone : public HitTest { public: BoundingCone(IObject* d, bool staticF) : HitTest(d, 0.0f, staticF, c_htsCone) { } Vector GetExtreme(const Vector& direction) { //On the cone portion of the cone if (direction.z >= (sqrt2 / 2.0f)) { return Vector(0.0f, 0.0f, GetRadius()); //Near the tip } else { //Near the edge ... return the corresponding point on the edge. float l = (float)sqrt(direction.x * direction.x + direction.y * direction.y); if (l < 0.01f) return Vector(0.0f, 0.0f, 0.0f); else { float xy = GetRadius() / l; return Vector(direction.x * xy, direction.y * xy, 0.0f); } } } }; #undef new class SingleHull { public: SingleHull(int nVertices, const Vector& center) : m_nVertices(nVertices), m_center(center) { } void* operator new(size_t size, int nVertices, int nAdjacencies) { return (void*)(::new char [size + sizeof(ConvexVertex) * nVertices + sizeof(ConvexVertex*) * (nAdjacencies + nVertices)]); } const ConvexVertex* GetExtremeVertex(const Vector& direction) const { return m_pcvExtremes[((direction.x < 0.0) ? 0 : 1) + ((direction.y < 0.0) ? 0 : 2) + ((direction.z < 0.0) ? 0 : 4)]; } void CalculateExtremeVertices(void) { static const float octants[][3] = { {-1.0f, -1.0f, -1.0f}, { 1.0f, -1.0f, -1.0f}, {-1.0f, 1.0f, -1.0f}, { 1.0f, 1.0f, -1.0f}, {-1.0f, -1.0f, 1.0f}, { 1.0f, -1.0f, 1.0f}, {-1.0f, 1.0f, 1.0f}, { 1.0f, 1.0f, 1.0f} }; //Pre-find the extreme vertices in each octant for (int octant = 0; (octant < 8); octant++) { const Vector& direction = *((Vector*)(octants[octant])); const ConvexVertex* pcv = vertex0(); double dotMax = direction * pcv->m_position; while (true) { const ConvexVertex* pcvNext = NULL; const ConvexVertex** ppcvAdjacent = pcv->m_adjacencies; do { double dot = direction * (*ppcvAdjacent)->m_position; if (dot > dotMax) { dotMax = dot; pcvNext = (*ppcvAdjacent); } } while (*(++ppcvAdjacent) != NULL); if (pcvNext) pcv = pcvNext; else { m_pcvExtremes[octant] = pcv; break; } } } } const Vector& GetCenter(void) const { return m_center; } private: const ConvexVertex* vertex0(void) const { return ((const ConvexVertex*)(((char*)this) + sizeof(*this))); } const ConvexVertex** adjacency0(void) const { return ((const ConvexVertex**)(vertex0() + m_nVertices)); } int m_nVertices; const ConvexVertex* m_pcvExtremes[8]; Vector m_center; friend class BoundingHull; friend class HitTest; }; class MultiHull : public MultiHullBase { public: MultiHull(int nHulls, float originalRadius, const Vector& ellipseEquation, float ellipseRadiusMultiplier) : MultiHullBase(ellipseEquation, originalRadius), m_nHulls(nHulls), //m_ellipseEquation(ellipseEquation), m_ellipseRadiusMultiplier(ellipseRadiusMultiplier) { } ~MultiHull(void) { for (int i = 0; (i < m_nHulls); i++) delete GetSingleHull(i); } void* operator new(size_t size, int nHulls) { return (void*)(::new char [size + sizeof(SingleHull*) * nHulls]); } int GetNhulls(void) const { return m_nHulls; } SingleHull* GetSingleHull(int hullID) const { assert (hullID >= 0); assert (hullID < m_nHulls); return hull0()[hullID]; } private: SingleHull** hull0(void) const { return ((SingleHull**)(((char*)this) + sizeof(*this))); } int m_nHulls; float m_ellipseRadiusMultiplier; //>= m_originalRadius //Vector m_ellipseEquation; //(x/ee.x)^2 + (y/ee.y)^2 + (z/ee.z)^2 <= 1 friend class BoundingHull; friend class HitTest; }; class BoundingHull : public HitTest { public: BoundingHull(IObject* d, bool staticF, const MultiHull* pMultiHull) : HitTest(d, pMultiHull->m_originalRadius, staticF, pMultiHull->GetNhulls()), m_pMultiHull(pMultiHull) { const ConvexVertex** pcvRecent = recentVertex0(); for (int i = pMultiHull->GetNhulls() - 1; (i >= 0); i--) pcvRecent[i] = pMultiHull->GetSingleHull(i)->vertex0(); } void* operator new(size_t size, const MultiHull* pMultiHull) { return (void*)(::new char [size + sizeof(ConvexVertex*) * pMultiHull->GetNhulls()]); } virtual void UpdateStaticBB(void) { HitTest::UpdateStaticBB(m_desiredRadius * m_pMultiHull->m_ellipseRadiusMultiplier); } Vector GetCenter(HitTestShape hts) const { assert (hts >= c_htsConvexHullMin); return m_pMultiHull->GetSingleHull(hts)->GetCenter() * m_scale; } Vector GetEllipseExtreme(const Vector& direction) { //Get the extreme point of an ellipse in direction //The ellipse equation is: (x/a)^2 + (y/b)^2 + (z/c)^2 <= 1 // //The normal to the ellipse at (x,y,z) is (x/a^2, y/b^2, z/c^2) //and the normal must be parallel to the direction. Therefore: // direction = Dxyz = k * (x/a^2, y/b^2, z/c^2) // x = a^2 Dx/k, y = b^2 Dy/k, z = c^2 Dz / k // and a^2 Dx^2 + b^2 Dy^2 + c^2 Dz^2 = k^2 double a2 = m_ellipseEquation.x * m_ellipseEquation.x; double b2 = m_ellipseEquation.y * m_ellipseEquation.y; double c2 = m_ellipseEquation.z * m_ellipseEquation.z; double rk = 1.0 / sqrt(a2 * direction.x * direction.x + b2 * direction.y * direction.y + c2 * direction.z * direction.z); Vector extreme((float)(direction.x * a2 * rk), (float)(direction.y * b2 * rk), (float)(direction.z * c2 * rk)); return extreme; } #pragma optimize ("p", on) Vector GetHullExtreme(int hullID, const Vector& direction) { assert (hullID >= 0); assert (hullID < m_pMultiHull->GetNhulls()); assert (direction.LengthSquared() >= 0.98f); assert (direction.LengthSquared() <= 1.02f); assert (m_pMultiHull); const ConvexVertex** ppcvRecent = &(recentVertex0()[hullID]); assert (ppcvRecent); const ConvexVertex* pcv = *ppcvRecent; assert (pcv); float dotMax = direction * pcv->m_position; if (dotMax < 0.0) { pcv = m_pMultiHull->GetSingleHull(hullID)->GetExtremeVertex(direction); dotMax = direction * pcv->m_position; } int n = 0; while (true) { //Hack to verify that FPU round-off error will not cause an infinite loop in retail n++; ZRetailAssert (n < 300); const ConvexVertex* pcvNext = NULL; const ConvexVertex** ppcvAdjacent = pcv->m_adjacencies; do { float dot = direction * (*ppcvAdjacent)->m_position; #ifdef _M_IX86 // should only be a problem on x86 CPUs __asm lea eax, [dot]; #endif if (dot > dotMax) { dotMax = dot; pcvNext = (*ppcvAdjacent); } } while (*(++ppcvAdjacent) != NULL); if (pcvNext) pcv = pcvNext; else { *ppcvRecent = pcv; return pcv->m_position * m_scale; } } } #pragma optimize ("", on) virtual void SetShape(HitTestShape shape) { //Never upgrade to anything more complex than our true shape HitTestShape s = (shape <= m_shapeTrue) ? shape : m_shapeTrue; if (s != m_shape) { m_shape = s; if (m_shape == c_htsEllipse) m_radius = m_desiredRadius * m_pMultiHull->m_ellipseRadiusMultiplier; else m_radius = m_desiredRadius; } } virtual void SetRadius(float r) { m_desiredRadius = r; m_scale = r / m_pMultiHull->m_originalRadius; m_ellipseEquation = m_scale * m_pMultiHull->GetEllipseEquation(); //Add a fudge factor to the bounding sphere for an ellipse //which is generally larger than the bounding sphere for either //a sphere or a convex hull. if (m_shape == c_htsEllipse) m_radius = r * m_pMultiHull->m_ellipseRadiusMultiplier; else m_radius = r; } virtual float GetScale(void) const { return m_scale; } virtual const Vector* GetEllipseEquation(void) const { return &m_ellipseEquation; } const Vector GetFrameOffset(const char* pszFrameName) const { return m_pMultiHull->GetFrameOffset(pszFrameName) * m_scale; } const Vector& GetFrameForward(const char* pszFrameName) const { return m_pMultiHull->GetFrameOffset(pszFrameName); } const FrameDataUTL* GetFrame(const char* pszFrameName) const { return m_pMultiHull->GetFrame(pszFrameName); } private: const ConvexVertex** recentVertex0(void) const { return ((const ConvexVertex**)(((char*)this) + sizeof(*this))); } float m_desiredRadius; float m_scale; Vector m_ellipseEquation; const MultiHull* m_pMultiHull; }; class CachedMultiHull { public: ~CachedMultiHull(void) { delete m_pMultiHull; } char m_name[c_cbName]; MultiHull* m_pMultiHull; friend bool operator!=(const CachedMultiHull& r1, const CachedMultiHull& r2) { return false; } }; typedef Slist_utl CachedList; typedef Slink_utl CachedLink; CachedList cachedMultiHulls; MultiHullBase* HitTest::Load(const char* pszFileName) { if ((!pszFileName) || (pszFileName[0] == '\0')) return NULL; MultiHull* pMultiHull = NULL; #ifndef DREAMCAST //Does the name exist in the cache? //It could exist but be a non-existant hull bool bFound = false; { for (CachedLink* pcl = cachedMultiHulls.first(); (pcl != NULL); pcl = pcl->next()) { const CachedMultiHull& cmh = pcl->data(); if (_stricmp(pszFileName, cmh.m_name) == 0) { pMultiHull = cmh.m_pMultiHull; bFound = true; break; } } } if (!bFound) { assert (pMultiHull == NULL); //First time we've seen this file ... read it in. char artwork[MAX_PATH]; HRESULT hr = UTL::getFile(pszFileName, ".cvh", artwork, true, true); if (hr == S_OK) //Don't bother to try and read a file that doesn't contain a valid convex hull { FILE* fileIn = fopen(artwork, "r"); if (fileIn) { const int c_maxLine = 512; char line[c_maxLine]; float radius; Vector ee; //ellipse equation float erm; if (fgets(line, c_maxLine, fileIn) && (sscanf(line, "%f %f %f %f %f", &radius, &ee.x, &ee.y, &ee.z, &erm) == 5)) { int nHulls; if (fgets(line, c_maxLine, fileIn) && (sscanf(line, "%d", &nHulls) == 1)) { assert (nHulls > 0); assert (nHulls < c_htsConvexHullMax); pMultiHull = new(nHulls) MultiHull(nHulls, radius, ee, erm); SingleHull** ppsh = pMultiHull->hull0(); for (int hullID = nHulls - 1; (hullID >= 0); hullID--) { int nVertices; int nAdjacencies; Vector center; if (fgets(line, c_maxLine, fileIn) && (sscanf(line, "%d %d %f %f %f", &nVertices, &nAdjacencies, &(center.x), &(center.y), &(center.z)) == 5)) { assert (nVertices > 0); assert (nAdjacencies > 0); //Same LHS nonsense as for vertices. center.x = -center.x; SingleHull* psh = new(nVertices, nAdjacencies) SingleHull(nVertices, center); *(ppsh++) = psh; //Hack alert: force a cast away from constness so that we can modify the vertex //as we read it in. ConvexVertex* pcv = (ConvexVertex*)(psh->vertex0()); const ConvexVertex** ppcvAdjacent = psh->adjacency0(); //Read in the vertices and their adjacency information. do { Vector xyz; int n; if (fgets(line, c_maxLine, fileIn) && (sscanf(line, "%f %f %f %d", &xyz.x, &xyz.y, &xyz.z, &n) == 4)) { assert (n > 0); //NYI hack, hack, hack ... objects are yaw'd 180 degrees so apply the same //transformation the vertices in the convex hull xyz.x = -xyz.x; //Initialize the vertex pcv->SetPosition(xyz); pcv->m_adjacencies = ppcvAdjacent; //Read in the adjacent vertex information do { int id; if (fscanf(fileIn, "%d ", &id) == 1) { nAdjacencies--; assert (nAdjacencies >= 0); *(ppcvAdjacent++) = psh->vertex0() + id; } else assert (false); } while (--n); *(ppcvAdjacent++) = NULL; } else assert (false); pcv++; } while (--nVertices); assert (nAdjacencies == 0); psh->CalculateExtremeVertices(); } } } } while (fgets(line, c_maxLine, fileIn)) { FrameLink* pfl = new FrameLink; FrameDataUTL* pfd = &(pfl->data()); assert (strlen(line) > 0); assert (strlen(line) < c_cbFrameName); line[strlen(line) - 1] = '\0'; //Trim off the \n strcpy(pfd->szName, line); ZVerify(fgets(line, c_maxLine, fileIn)); sscanf(line, "%f %f %f", &(pfd->position.x), &(pfd->position.y), &(pfd->position.z)); ZVerify(fgets(line, c_maxLine, fileIn)); sscanf(line, "%f %f %f", &(pfd->forward.x), &(pfd->forward.y), &(pfd->forward.z)); //Hack ... objects are flipped when they are read in so do it for the frames as well. pfd->position.x = -pfd->position.x; //Forward directions are completely reversed (and need X flipped ... so flip y & z) pfd->forward.y = -pfd->forward.y; pfd->forward.z = -pfd->forward.z; pMultiHull->m_frames.last(pfl); } fclose(fileIn); } } //Create a cache entry for this convex hull file (whether or not we were //actually able to load the .cvh file) { CachedLink* pcl = new CachedLink; CachedMultiHull& cmh = pcl->data(); assert (strlen(pszFileName) < c_cbName); strcpy(cmh.m_name, pszFileName); cmh.m_pMultiHull = pMultiHull; //Put it at the front of the list (since we might read another one in real soon). cachedMultiHulls.first(pcl); } } #endif return pMultiHull; } HitTest* HitTest::Create(const char* pszFileName, IObject* data, bool staticF, HitTestShape htsDefault) { HitTest* pHitTest = NULL; #ifndef DREAMCAST if (pszFileName) { //An unsafe upcast ... but we know better. MultiHull* pMultiHull = (MultiHull*)Load(pszFileName); if (pMultiHull) { pHitTest = new (pMultiHull) BoundingHull(data, staticF, pMultiHull); } } #endif //dreamcast if (!pHitTest) { switch (htsDefault) { case c_htsSphere: pHitTest = (HitTest*)(new BoundingSphere(data, staticF)); break; case c_htsPoint: pHitTest = (HitTest*)(new BoundingPoint(data, staticF)); break; case c_htsCone: pHitTest = (HitTest*)(new BoundingCone(data, staticF)); break; default: assert (false); } } return pHitTest; } Vector HitTest::GetMinExtreme(HitTestShape hts, const Vector& position, const Orientation& orientation, const Vector& direction) { switch (hts) { case c_htsPoint: return position; case c_htsEllipse: return position + ((BoundingHull*)this)->GetEllipseExtreme(orientation.TimesInverse(-direction)) * orientation; case c_htsCone: return position + ((BoundingCone*)this)->GetExtreme(orientation.TimesInverse(-direction)) * orientation; case c_htsSphere: return position - direction * m_radius; default: return position + ((BoundingHull*)this)->GetHullExtreme(hts, orientation.TimesInverse(-direction)) * orientation; } } Vector HitTest::GetMaxExtreme(HitTestShape hts, const Vector& direction) { switch (hts) { case c_htsPoint: return Vector::GetZero(); case c_htsEllipse: return ((BoundingHull*)this)->GetEllipseExtreme(direction); case c_htsCone: return ((BoundingCone*)this)->GetExtreme(direction); case c_htsSphere: return direction * m_radius; default: return ((BoundingHull*)this)->GetHullExtreme(hts, direction); } } static bool DoGilbert(HitTest* phtObjectA, HitTestShape htsA, const Vector& positionStart, const Vector& positionStop, const Vector& dV, const Orientation& orientationHullA, HitTest* phtObjectB, HitTestShape htsB, Vector simplex[4]); bool HitTest::HullCollide(float* tStart, float tMax, HitTest* phtHullA, HitTestShape* phtsA, HitTest* phtHullB, HitTestShape* phtsB, const Vector& dP, const Vector& dV) { assert (tStart); assert (phtHullA); assert (phtHullB); //In general, phtHullB is more complex than phtHullA ... so optimize things to minimize the manipulations of phtHullB //In particular, re-orient everything so that we are in B's local coordinate space (dP & dV are aready wrt B). const Orientation& oB = phtHullB->GetOrientation(); //A common special case is a c_htsPoint vs. c_htsEllipse, so test that explicitly if ((phtHullA->m_shape == c_htsPoint) && (phtHullB->m_shape == c_htsEllipse)) { Vector p = oB.TimesInverse(dP - *tStart * dV); //Point vs. ellipse ... distort the coordinate system based on the ellipse equation of phtHullB (which we can do since we //are in phtHullB's local coordinate system). const Vector& ee = *(phtHullB->GetEllipseEquation()); //Now ... when does a point starting at p and moving -v intersect a sphere with radius 1.0 around the origin //P(t) = p - v * t; P(t)^2 = 1 = p*p - 2v*p*t + v*v*t^2 // //v*v*t^2 - 2v*p*t + p*p-1 = 0 //(v*v/2) t^2 - v*p t + ((p*p-1)/2) = 0 // //(a/2) t^2 - b t + (c/2) = 0 // t = (b +- sqrt(b*b-4(a/2)(c/2))) / (2 * (a/2)) = (b +- sqrt(b*b-ac)) / a p.x /= ee.x; p.y /= ee.y; p.z /= ee.z; double c = p * p - 1.0; if (c <= 0.0) { //Special case ... point is on or inside the ellipse at the start of the interval *phtsA = phtHullA->m_shape; *phtsB = phtHullB->m_shape; return true; } //c > 0 Vector v = oB.TimesInverse(dV); v.x /= ee.x; v.y /= ee.y; v.z /= ee.z; double b = v * p; if (b > 0.0) { //The point is moving closer to the ellipse ... continue double a = v * v; //>= 0 double bac = b*b - a * c; //a*c >= 0.0 so bac <= b*b if (bac >= 0.0) { //Starting from *tStart so ... calculate time time of collision appropriately float t = *tStart + float((b - sqrt(bac)) / a); if (t <= tMax) { *tStart = t; *phtsA = phtHullA->m_shape; *phtsB = phtHullB->m_shape; return true; } } } return false; } else { Vector p = oB.TimesInverse(dP); Vector v = oB.TimesInverse(dV); const Orientation& oA = phtHullA->GetOrientation(); Orientation o = oA.TimesInverse(oB); // == oA * oB^-1 const double c_maxDelta = 0.25; double speed = dV.Length(); //Hack alert ... only phtHullB should be subject to the on the fly adjustment //of the hit test shape ... so fiddle with their shape. HitTestShape htsA = phtHullA->m_shape; HitTestShape htsB = ((phtHullB->m_pvUseTrueShapeSelf == NULL) || (phtHullB->m_pvUseTrueShapeSelf != phtHullA->m_pvUseTrueShapeOther)) ? phtHullB->m_shape : phtHullB->m_shapeTrue; HitTestShape htsAcurrent = (htsA < 0) ? htsA : 0; float tOriginalStart = *tStart; bool bCollision = false; do { HitTestShape htsBcurrent = (htsB < 0) ? htsB : 0; do { if (HitTest::IntervalCollide(tOriginalStart, tMax, (speed == 0.0) ? FLT_MAX : (float)(c_maxDelta / speed), phtHullA, htsAcurrent, phtHullB, htsBcurrent, p, v, o, tStart)) { bCollision = true; *phtsA = htsAcurrent; *phtsB = htsBcurrent; tMax = *tStart; //No point in worrying about collisions that happen after colliding with another part } } while (++htsBcurrent < htsB); } while (++htsAcurrent < htsA); return bCollision; } } bool HitTest::IntervalCollide(float tStart, float tStop, float maxDeltaT, HitTest* phtHullA, HitTestShape htsA, HitTest* phtHullB, HitTestShape htsB, const Vector& dP, const Vector& dV, const Orientation& orientationA, float* tCollision) { float tMiddle = (tStart + tStop) / 2.0f; //Do the two convex hulls intersect anywhere along the interval between *tCollision and tMax? Vector direction = dP - tMiddle * dV; if (htsA >= c_htsConvexHullMin) direction += phtHullA->GetCenter(htsA) * orientationA; if (htsB >= c_htsConvexHullMin) direction -= phtHullB->GetCenter(htsB); if ((direction.x == 0.0f) && (direction.y == 0.0f) && (direction.z == 0.0f)) return true; direction.SetNormalize(); Vector positionStart = (dP - tStart * dV); Vector positionStop = (dP - tStop * dV); //Try 4 times, saving each result to see if we //can find a plane that clearly puts the sphere //outside the hull const int c_maxAttempts = 4; Vector extremesHullB[c_maxAttempts]; Vector extremesHullA[c_maxAttempts]; int attempt = 0; while (true) { //For hull A, the extreme is found by takeing the extreme in the given direction and then //offsetting it by the position at start or the end of the interval (which ever is appropriate: //there are two sets of negation cancelling out here). extremesHullA[attempt] = phtHullA->GetMinExtreme(htsA, (direction * dV) >= 0.0f ? positionStop : positionStart, orientationA, direction); extremesHullB[attempt] = phtHullB->GetMaxExtreme(htsB, direction); Vector delta = extremesHullA[attempt] - extremesHullB[attempt]; double distance = (delta * direction); if (distance > 0.0) { //There is no collision anywyare along the interval ... bye return false; } else if (attempt++ >= c_maxAttempts - 1) break; //Didn't find a good separating plane ... munge direction in the hopes of finding a better one //reflect direction around the vector from the extreme to the sphere //Get the vector from direction to its projection onto -delta direction -= delta * (2.0f * (direction * delta) / delta.LengthSquared()); assert (direction.Length() >= 0.98f); assert (direction.Length() <= 1.02f); } //We made 4 attempts to find a separating plane and failed. //Invoke Gilbert's algorithm. Vector simplex[4]; { int i = 0; do simplex[i] = extremesHullA[i] - extremesHullB[i]; while (i++ < 3); } if (!DoGilbert(phtHullA, htsA, positionStart, positionStop, dV, orientationA, phtHullB, htsB, simplex)) { //No collision ... also bye return false; } //We have a collision somewhere along the interval ... if (tStop - tStart <= maxDeltaT) { //The interval is small enough that we really do not care any more. *tCollision = tMiddle; return true; } else //split the interval in two and try again return IntervalCollide(tStart, tMiddle, maxDeltaT, phtHullA, htsA, phtHullB, htsB, dP, dV, orientationA, tCollision) || IntervalCollide(tMiddle, tStop, maxDeltaT, phtHullA, htsA, phtHullB, htsB, dP, dV, orientationA, tCollision); } static int closest_vertex(int v0, const Vector p[], //double lambda[], int index[], Vector* cp) { //lambda[0] = 1; index[0] = v0; *cp = p[v0]; return 1; } static int closest_edge(double e0, double e1, int ie0, int ie1, const Vector p[], int index[], Vector* cp) { double esum_inv = 1.0f/(e0+e1); float lambda[2]; lambda[0] = (float)(e0*esum_inv); lambda[1] = (float)(e1*esum_inv); index[0] = ie0; index[1] = ie1; *cp = lambda[0] * p[ie0] + lambda[1] * p[ie1]; return 2; } static int closest_face(double f0, double f1, double f2, int if0, int if1, int if2, const Vector p[], //double lambda[], int index[], Vector* cp) { double fsum_inv = 1.0 / (f0+f1+f2); float lambda[3]; lambda[0] = (float)(f0*fsum_inv); lambda[1] = (float)(f1*fsum_inv); lambda[2] = (float)(f2*fsum_inv); index[0] = if0; index[1] = if1; index[2] = if2; *cp = lambda[0] * p[if0] + lambda[1] * p[if1] + lambda[2] * p[if2]; return 3; } /* Johnson algorithm for tetrahedron Johnson's algorithm for finding the closest point to the origin on a simplex (3d). Inputs: p -- array of n 3d points forming the simplex n -- number of points, must be 1, 2, 3, or 4. Outputs: cp -- closest point to origin on simplex index -- indices of simplex points forming feature closest to origin {in increasing order] lambda -- coefficients of above indexed points forming cp Each coefficient is nonnegative and their sum is 1. Returns: number of simplex points forming closest feature: 1 = vertex 2 = edge 3 = face 4 = tetrahedron (inside simplex interior) The return value is always <= n and >= 1. */ static int Johnson2(const Vector p[2], Vector* cp, int index[]) { Vector d = p[1] - p[0]; double l0 = p[0] * d; if (l0 >= 0) { *cp = p[0]; index[0] = 0; return 1; } double l1 = p[1] * d; //Note ... sense is reversed wrt l0 if (l1 <= 0) { *cp = p[1]; index[0] = 1; return 1; } index[0] = 0; index[1] = 1; *cp = p[0] + (float)(l0 / (l0 - l1)) * d; return 2; } /* #define DOT(a,b) ( (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] ) static int jsny3(double *p,double cp[3], int index[]) { double d[3][3]; // array of dot products d[i][j] = double d2[3][2]; // array of D_k{i,j} encoded as [l][0 if k=i, 1 if k=j] i < j, k in {i,j} // where l=0 -> {0,1}, l=1 -> {1,2}, l=2 -> {0,2} double d3[3]; // array of D_l{i,j,k} encoded as [l], i < j < k, l in {i,j,k} register double *pi,*pj; register int i,j; //double min; //int n_min; // compute dot products pi = p; for (i = 0; i < 3; i++) { pj = pi; for (j = i; j < 3; j++) { d[i][j] = d[j][i] = DOT(pi,pj); pj += 3; } pi += 3; } // compute D2 d2[0][0] = d[1][1] - d[1][0]; d2[0][1] = d[0][0] - d[0][1]; d2[1][0] = d[2][2] - d[2][1]; d2[1][1] = d[1][1] - d[1][2]; d2[2][0] = d[2][2] - d[2][0]; d2[2][1] = d[0][0] - d[0][2]; // compute D3 d3[0] = d2[1][0]*(d[1][1]-d[1][0]) + d2[1][1]*(d[2][1]-d[2][0]); d3[1] = d2[2][0]*(d[0][0]-d[0][1]) + d2[2][1]*(d[2][0]-d[2][1]); d3[2] = d2[0][0]*(d[0][0]-d[0][2]) + d2[0][1]*(d[1][0]-d[1][2]); debugf("jsny3\n"); { debugf("p[]\n"); for (int i = 0; (i < 3); i++) debugf("%f %f %f\n", p[i*3+0], p[i*3+1], p[i*3+2]); debugf("d[][]\n"); for (int j = 0; (j < 3); j++) debugf("%f %f %f\n", d[j][0], d[j][1], d[j][2]); debugf("d2[][]\n"); for (int k = 0; (k < 3); k++) debugf("%f %f\n", d2[k][0], d2[k][1]); debugf("d3\n"); debugf("%f %f %f\n", d3[0], d3[1], d3[2]); } return 0; } */ static int Johnson3(const Vector p[3], Vector* cp, int index[]) { //Mappings between all possible subsets of the vertices and a //unique subset ID. // subset1[i] ({Pi}) // subset2[i][j] ({Pi, Pj}) i != j // subset3 ({P0, P1, P2}) static const int subset1[3] = { 0, 1, 2}; static const int subset2[3][3] = {{-1, 3, 4}, { 3, -1, 5}, { 4, 5, -1}}; static const int subset3 = 6; double dp[3][3]; { for (int i = 0; (i < 3); i++) for (int j = i; (j < 3); j++) dp[i][j] = dp[j][i] = (double)(p[i].x) * (double)(p[j].x) + (double)(p[i].y) * (double)(p[j].y) + (double)(p[i].z) * (double)(p[j].z); } double delta[3][subset3 + 1]; //Generic form: delta[i][{Pi}] = 1.0 // but don't bother { //Generic form: delta[i][{Pj} | {Pi}] = Pj * (Pj - Pi) [i != j] // = dp[j][j] - dp[j][i] #define SetDelta(i,j) delta[i][subset2[j][i]] = dp[j][j] - dp[j][i] SetDelta(1, 0); SetDelta(2, 0); SetDelta(0, 1); SetDelta(2, 1); SetDelta(0, 2); SetDelta(1, 2); #undef SetDelta } { //Generic form: delta[i][{Pj, Pk} | {Pi}] = delta[j][{Pj, Pk}] * (Pj * (Pj - Pi)) + [i != j, k; j < k] // delta[k][{Pj, Pk}] * (Pk * (Pj - Pi)) // = delta[j][{Pj, Pk}] * (dp[j][j] - dp[j][i]) + // delta[k][{Pj, Pk}] * (dp[k][j] - dp[k][i]) // i != j, k; j < k; x != i, j, k #define SetDelta(i, j, k) delta[i][subset3] = (delta[j][subset2[j][k]] * (dp[j][j] - dp[j][i])) + \ (delta[k][subset2[j][k]] * (dp[k][j] - dp[k][i])) SetDelta(2, 0, 1); SetDelta(1, 0, 2); SetDelta(0, 1, 2); #undef SetDelta } //Now that we've done all this ... see if we have a solution //We have a valid solution if: // delta[i][S] > 0 (for all i in S) // delta[j][S | {Pj}] <= 0 (for all j not in S) /* { debugf("p[]\n"); for (int i = 0; (i < 3); i++) debugf("%f %f %f\n", p[i].x, p[i].y, p[i].z); debugf("dp[][]\n"); for (int j = 0; (j < 3); j++) debugf("%f %f %f\n", dp[j][0], dp[j][1], dp[j][2]); debugf("delta[][]\n"); debugf("%f %f\n", delta[0][subset2[0][1]], delta[1][subset2[0][1]]); debugf("%f %f\n", delta[1][subset2[1][2]], delta[2][subset2[1][2]]); debugf("%f %f\n", delta[2][subset2[0][2]], delta[2][subset2[0][2]]); debugf("d3\n"); debugf("%f %f %f\n", delta[0][subset3], delta[1][subset3], delta[2][subset3]); } */ //Inside for the triangle. The last clause of the test is degenerate //since there is no Pj not in S if ((delta[0][subset3] > 0.0) && (delta[1][subset3] > 0.0) && (delta[2][subset3] > 0.0)) { //Yes return closest_face(delta[0][subset3], delta[1][subset3], delta[2][subset3], 0, 1, 2, p, index, cp); } { //Test for each vertex. The first clause of the test is degerate //since delta[i][S[i]] = 1 //Test for vertex i #define TestDelta(i, j, k) if ((delta[i][subset2[j][i]] <= 0.0) && \ (delta[i][subset2[k][i]] <= 0.0)) \ return closest_vertex(i, p, index, cp); TestDelta(0, 1, 2); TestDelta(1, 0, 2); TestDelta(2, 0, 1); #undef TestDelta } { //Test for each edge i, j // delta[i][subset2[i][j]] > 0 // delta[j][subset2[i][j]] > 0 // // delta[k][subset3[l]] < 0 // delta[l][subset3[k]] < 0 #define TestDelta(i, j, k) if ((delta[i][subset2[i][j]] > 0.0) && \ (delta[j][subset2[i][j]] > 0.0) && \ (delta[k][subset3] <= 0.0)) \ return closest_edge(delta[i][subset2[i][j]], \ delta[j][subset2[i][j]], \ i, j, p, index, cp) TestDelta(0, 1, 2); TestDelta(0, 2, 1); TestDelta(1, 2, 0); #undef TestDelta } { //Test for each face i, j, k // delta[i][subset3[l]] > 0 // delta[j][subset3[l]] > 0 // delta[k][subset3[l]] > 0 // // delta[l][subset4] <= 0.0f #define TestDelta(i, j, k) if ((delta[i][subset3] > 0.0) && \ (delta[j][subset3] > 0.0) && \ (delta[k][subset3] > 0.0)) \ return closest_face(delta[i][subset3], \ delta[j][subset3], \ delta[k][subset3], \ i, j, k, p, index, cp) TestDelta(0, 1, 2); #undef TestDelta } //If we haven't found a solution so far (round off or some such) ... int nMin; double dMin2 = HUGE_VAL; { //Try the vertices int v; for (int i = 0; (i < 3); i++) { if (dp[i][i] < dMin2) { dMin2 = dp[i][i]; v = i; } } nMin = closest_vertex(v, p, index, cp); } { //Try the edges for (int i = 0; (i < 2); i++) { for (int j = i + 1; (j < 3); j++) { if ((delta[i][subset2[i][j]] > 0.0) && (delta[j][subset2[i][j]] > 0.0)) { double d = (delta[i][subset2[i][j]] * dp[i][i] + delta[j][subset2[i][j]] * dp[j][i]) / (delta[i][subset2[i][j]] + delta[j][subset2[i][j]]); if (d < dMin2) { dMin2 = d; nMin = closest_edge(delta[i][subset2[i][j]], delta[j][subset2[i][j]], i, j, p, index, cp); } } } } } { if ((delta[0][subset3] > 0.0) && (delta[1][subset3] > 0.0) && (delta[2][subset3] > 0.0)) { double d = (delta[0][subset3] * dp[0][0] + delta[1][subset3] * dp[1][0] + delta[2][subset3] * dp[2][0]) / (delta[0][subset3] + delta[1][subset3] + delta[2][subset3]); if (d < dMin2) { dMin2 = d; nMin = closest_face(delta[0][subset3], delta[1][subset3], delta[2][subset3], 0, 1, 2, p, index, cp); } } } return nMin; } static int Johnson4(const Vector p[4], Vector* cp, int index[]) { //Mappings between all possible subsets of the vertices and a //unique subset ID. // subset1[i] ({Pi}) // subset2[i][j] ({Pi, Pj}), i != j // subset3[i] ({Pj, Pk, Pl}), i != j; i != k; i != l // subset4 ({P0, P1, P2, P3}) static const int subset1[4] = { 0, 1, 2, 3}; static const int subset2[4][4] = {{-1, 4, 5, 6}, { 4, -1, 7, 8}, { 5, 7, -1, 9}, { 6, 8, 9, -1}}; static const int subset3[4] = {10, 11, 12, 13}; static const int subset4 = 14; double dp[4][4]; { for (int i = 0; (i < 4); i++) for (int j = i; (j < 4); j++) dp[i][j] = dp[j][i] = (double)(p[i].x) * (double)(p[j].x) + (double)(p[i].y) * (double)(p[j].y) + (double)(p[i].z) * (double)(p[j].z); } double delta[4][subset4 + 1]; //Generic form: delta[i][{Pi}] = 1.0 // but don't bother { //Generic form: delta[i][{Pj} | {Pi}] = Pj * (Pj - Pi) [i != j] // = dp[j][j] - dp[j][i] #define SetDelta(i,j) delta[i][subset2[j][i]] = dp[j][j] - dp[j][i] SetDelta(1, 0); SetDelta(2, 0); SetDelta(3, 0); SetDelta(0, 1); SetDelta(2, 1); SetDelta(3, 1); SetDelta(0, 2); SetDelta(1, 2); SetDelta(3, 2); SetDelta(0, 3); SetDelta(1, 3); SetDelta(2, 3); #undef SetDelta } { //Generic form: delta[i][{Pj, Pk} | {Pi}] = delta[j][{Pj, Pk}] * (Pj * (Pj - Pi)) + [i != j, k; j < k] // delta[k][{Pj, Pk}] * (Pk * (Pj - Pi)) // // = delta[j][{Pj, Pk}] * (dp[j][j] - dp[j][i]) + // delta[k][{Pj, Pk}] * (dp[k][j] - dp[k][i]) // i != j, k; j < k; x != i, j, k #define SetDelta(i, j, k, x) delta[i][subset3[x]] = (delta[j][subset2[j][k]] * (dp[j][j] - dp[j][i])) + \ (delta[k][subset2[j][k]] * (dp[k][j] - dp[k][i])) SetDelta(2, 0, 1, 3); SetDelta(3, 0, 1, 2); SetDelta(1, 0, 2, 3); SetDelta(3, 0, 2, 1); SetDelta(1, 0, 3, 2); SetDelta(2, 0, 3, 1); SetDelta(0, 1, 2, 3); SetDelta(3, 1, 2, 0); SetDelta(0, 1, 3, 2); SetDelta(2, 1, 3, 0); SetDelta(0, 2, 3, 1); SetDelta(1, 2, 3, 0); #undef SetDelta } { //Generic form: delta[i][{Pj, Pk, Pl} | {Pi}] = delta[j][{Pj, Pk, Pl}] * (Pj * (Pj - Pi)) + [i != j, k, l; j < k, l, k != l] // delta[k][{Pj, Pk, Pl}] * (Pk * (Pj - Pi)) + // delta[l][{Pj, Pk, Pl}] * (Pl * (Pj - Pi)) // = delta[j][{Pj, Pk, Pl}] * (dp[j][j] - dp[j][i]) + // = delta[j][{Pj, Pk, Pl}] * (dp[k][j] - dp[k][i]) + // = delta[j][{Pj, Pk, Pl}] * (dp[k][j] - dp[l][i]) // i != j, k, l; j < k, l, k != l #define SetDelta(i, j, k, l) delta[i][subset4] = delta[j][subset3[i]] * (dp[j][j] - dp[j][i]) + \ delta[k][subset3[i]] * (dp[k][j] - dp[k][i]) + \ delta[l][subset3[i]] * (dp[l][j] - dp[l][i]) SetDelta(0, 1, 2, 3); SetDelta(1, 0, 2, 3); SetDelta(2, 0, 1, 3); SetDelta(3, 0, 1, 2); #undef SetDelta } //Now that we've done all this ... see if we have a solution //We have a valid solution if: // delta[i][S] > 0 (for all i in S) // delta[j][S | {Pj}] <= 0 (for all j not in S) //Inside for the tetrahedron. The last clause of the test is degenerate //since there is no Pj not in S if ((delta[0][subset4] > 0.0) && (delta[1][subset4] > 0.0) && (delta[2][subset4] > 0.0) && (delta[3][subset4] > 0.0)) { //Yes *cp = Vector::GetZero(); return 4; } { //Test for each vertex. The first clause of the test is degerate //since delta[i][S[i]] = 1 //Test for vertex i #define TestDelta(i, j, k, l) if ((delta[i][subset2[j][i]] <= 0.0) && \ (delta[i][subset2[k][i]] <= 0.0) && \ (delta[i][subset2[l][i]] <= 0.0)) \ return closest_vertex(i, p, index, cp); TestDelta(0, 1, 2, 3); TestDelta(1, 0, 2, 3); TestDelta(2, 0, 1, 3); TestDelta(3, 0, 1, 2); #undef TestDelta } { //Test for each edge i, j // delta[i][subset2[i][j]] > 0 // delta[j][subset2[i][j]] > 0 // // delta[k][subset3[l]] < 0 // delta[l][subset3[k]] < 0 #define TestDelta(i, j, k, l) if ((delta[i][subset2[i][j]] > 0.0) && \ (delta[j][subset2[i][j]] > 0.0) && \ (delta[k][subset3[l]] <= 0.0) && \ (delta[l][subset3[k]] <= 0.0)) \ return closest_edge(delta[i][subset2[i][j]], \ delta[j][subset2[i][j]], \ i, j, p, index, cp) TestDelta(0, 1, 2, 3); TestDelta(0, 2, 1, 3); TestDelta(0, 3, 1, 2); TestDelta(1, 2, 0, 3); TestDelta(1, 3, 0, 2); TestDelta(2, 3, 0, 1); #undef TestDelta } { //Test for each face i, j, k // delta[i][subset3[l]] > 0 // delta[j][subset3[l]] > 0 // delta[k][subset3[l]] > 0 // // delta[l][subset4] <= 0.0f #define TestDelta(i, j, k, l) if ((delta[i][subset3[l]] > 0.0) && \ (delta[j][subset3[l]] > 0.0) && \ (delta[k][subset3[l]] > 0.0) && \ (delta[l][subset4] <= 0.0)) \ return closest_face(delta[i][subset3[l]], \ delta[j][subset3[l]], \ delta[k][subset3[l]], \ i, j, k, p, index, cp) TestDelta(0, 1, 2, 3); TestDelta(0, 1, 3, 2); TestDelta(0, 2, 3, 1); TestDelta(1, 2, 3, 0); #undef TestDelta } //If we haven't found a solution so far (round off or some such) ... int nMin; double dMin2 = HUGE_VAL; { //Try the vertices int v; for (int i = 0; (i < 4); i++) { if (dp[i][i] < dMin2) { dMin2 = dp[i][i]; v = i; } } nMin = closest_vertex(v, p, index, cp); } { //Try the edges for (int i = 0; (i < 3); i++) { for (int j = i + 1; (j < 4); j++) { if ((delta[i][subset2[i][j]] > 0.0) && (delta[j][subset2[i][j]] > 0.0)) { double d = (delta[i][subset2[i][j]] * dp[i][i] + delta[j][subset2[i][j]] * dp[j][i]) / (delta[i][subset2[i][j]] + delta[j][subset2[i][j]]); if (d < dMin2) { dMin2 = d; nMin = closest_edge(delta[i][subset2[i][j]], delta[j][subset2[i][j]], i, j, p, index, cp); } } } } } { { //Try the faces const int ids[4][4] = {{0, 1, 2, 3}, {0, 1, 3, 2}, {0, 2, 3, 1}, {1, 2, 3, 0}}; for (int f = 0; (f < 4); f++) { int i = ids[f][0]; int j = ids[f][1]; int k = ids[f][2]; int l = ids[f][3]; if ((delta[i][subset3[l]] > 0.0) && (delta[j][subset3[l]] > 0.0) && (delta[k][subset3[l]] > 0.0)) { double d = (delta[i][subset3[l]] * dp[i][i] + delta[j][subset3[l]] * dp[j][i] + delta[k][subset3[l]] * dp[k][i]) / (delta[i][subset3[l]] + delta[j][subset3[l]] + delta[k][subset3[l]]); if (d < dMin2) { dMin2 = d; nMin = closest_face(delta[i][subset3[l]], delta[j][subset3[l]], delta[k][subset3[l]], i, j, k, p, index, cp); } } } } } return nMin; } int Johnson(int n, const Vector p[], Vector* cp, int index[]) { switch (n) { case 1: { *cp = p[0]; index[0] = 0; return 1; } case 2: { return Johnson2(p, cp, index); } case 3: { return Johnson3(p, cp, index); } default: { return Johnson4(p, cp, index); } } } static bool DoGilbert(HitTest* phtHullA, HitTestShape htsA, const Vector& positionStart, const Vector& positionStop, const Vector& dV, const Orientation& orientationHullA, HitTest* phtHullB, HitTestShape htsB, Vector simplex[4]) { int nVertices = 3; //Always start with four (but the three is incremented before it is used). static const int c_maxHistory = 100; static Vector vertexHistory[c_maxHistory]; vertexHistory[0] = simplex[0]; vertexHistory[1] = simplex[1]; vertexHistory[2] = simplex[2]; vertexHistory[3] = simplex[3]; int vertexID = 3; Vector cp; Vector delta; while (true) { int indices[3]; int nMin = Johnson(++nVertices, simplex, &cp, indices); if (nMin == 4) { return true; } //Find a new extreme point { double l2 = cp.LengthSquared(); if (l2 < epsilon * epsilon) return true; cp /= (float)sqrt(l2); } Vector v0 = phtHullA->GetMinExtreme(htsA, (cp * dV >= 0.0f) ? positionStop : positionStart, orientationHullA, cp) - phtHullB->GetMaxExtreme(htsB, cp); if ((v0 * cp) > 0.0) { //Found a separating plane return false; } //Have we been here before? { for (int i = vertexID; (i >= 0); i--) { if ((vertexHistory[i] - v0).LengthSquared() < epsilon * epsilon) { //A duplicate point ... so the simplex will not contain the origin. return false; } } assert (vertexID < c_maxHistory - 1); vertexHistory[++vertexID] = v0; } //Otherwise ... eliminate all the points from the simplex //that are not in the index array (assume the index array //is sorted). { int i = 0; do { if (indices[i] != i) { assert (indices[i] > i); simplex[i] = simplex[indices[i]]; } } while (++i < nMin); nVertices = i; } //Replace the last vertex in the simplex array with new vertex simplex[nVertices] = v0; } return false; }