Initial commit

Torque/SDK/engine/collision/extrudedPolyList.cc

@@ -0,0 +1,492 @@
+//-----------------------------------------------------------------------------
+// Torque Game Engine
+// Copyright (C) GarageGames.com, Inc.
+//-----------------------------------------------------------------------------
+
+#include "platform/platform.h"
+#include "dgl/dgl.h"
+#include "math/mMath.h"
+#include "console/console.h"
+#include "collision/extrudedPolyList.h"
+#include "collision/polyhedron.h"
+#include "collision/collision.h"
+
+
+const F32 sgFrontEpsilon = 0.01;
+
+//----------------------------------------------------------------------------
+
+// use table for U64 shifts of the form: 1 << N
+// because compiler makes it a function call if done directly...
+U32 U32leftShift[33] =
+{
+   U32(1) << 0,
+   U32(1) << 1,
+   U32(1) << 2,
+   U32(1) << 3,
+   U32(1) << 4,
+   U32(1) << 5,
+   U32(1) << 6,
+   U32(1) << 7,
+   U32(1) << 8,
+   U32(1) << 9,
+
+   U32(1) << 10,
+   U32(1) << 11,
+   U32(1) << 12,
+   U32(1) << 13,
+   U32(1) << 14,
+   U32(1) << 15,
+   U32(1) << 16,
+   U32(1) << 17,
+   U32(1) << 18,
+   U32(1) << 19,
+
+   U32(1) << 20,
+   U32(1) << 21,
+   U32(1) << 22,
+   U32(1) << 23,
+   U32(1) << 24,
+   U32(1) << 25,
+   U32(1) << 26,
+   U32(1) << 27,
+   U32(1) << 28,
+   U32(1) << 29,
+
+   U32(1) << 30,
+   U32(1) << 31,
+   U32(0) // one more for good measure
+};
+
+// Minimum distance from a face
+F32 ExtrudedPolyList::FaceEpsilon = 0.01;
+
+// Value used to compare collision times
+F32 ExtrudedPolyList::EqualEpsilon = 0.0001;
+
+ExtrudedPolyList::ExtrudedPolyList()
+{
+   VECTOR_SET_ASSOCIATION(mVertexList);
+   VECTOR_SET_ASSOCIATION(mIndexList);
+   VECTOR_SET_ASSOCIATION(mExtrudedList);
+   VECTOR_SET_ASSOCIATION(mPlaneList);
+   VECTOR_SET_ASSOCIATION(mPolyPlaneList);
+
+   mVelocity.set(0,0,0);
+   mIndexList.reserve(128);
+   mVertexList.reserve(64);
+   mPolyPlaneList.reserve(64);
+   mPlaneList.reserve(64);
+   mCollisionList = 0;
+}
+
+ExtrudedPolyList::~ExtrudedPolyList()
+{
+}
+
+
+//----------------------------------------------------------------------------
+
+bool ExtrudedPolyList::isEmpty() const
+{
+   return mCollisionList->count == 0;
+}
+
+
+//----------------------------------------------------------------------------
+
+void ExtrudedPolyList::extrude(const Polyhedron& pt, const VectorF& vector)
+{
+   // Clear state
+   mIndexList.clear();
+   mVertexList.clear();
+   mPlaneList.clear();
+   mPolyPlaneList.clear();
+   mFaceShift = 0;
+
+   // Determine which faces will be extruded.
+   mExtrudedList.setSize(pt.planeList.size());
+   for (U32 f = 0; f < pt.planeList.size(); f++) {
+      const PlaneF& face = pt.planeList[f];
+      ExtrudedFace& eface = mExtrudedList[f];
+      F32 dot = mDot(face,vector);
+      eface.active = dot > EqualEpsilon;
+      if (eface.active) {
+         eface.faceShift = FaceEpsilon / dot;
+         eface.maxDistance = dot;
+         eface.plane = face;
+         eface.planeMask = U32leftShift[mPlaneList.size()]; // U64(1) << mPlaneList.size();
+
+         // Add the face as a plane to clip against.
+         mPlaneList.increment(2);
+         PlaneF* plane = mPlaneList.end() - 2;
+         plane[0] = plane[1] = face;
+         plane[0].invert();
+      }
+   }
+
+   // Produce extruded planes for bounding and internal edges
+   for (U32 e = 0; e < pt.edgeList.size(); e++) {
+      Polyhedron::Edge const& edge = pt.edgeList[e];
+      ExtrudedFace& ef1 = mExtrudedList[edge.face[0]];
+      ExtrudedFace& ef2 = mExtrudedList[edge.face[1]];
+      if (ef1.active || ef2.active) {
+
+         // Assumes that the edge points are clockwise
+         // for face[0].
+         const Point3F& p1 = pt.pointList[edge.vertex[1]];
+         const Point3F &p2 = pt.pointList[edge.vertex[0]];
+         Point3F p3 = p2 + vector;
+
+         mPlaneList.increment(2);
+         PlaneF* plane = mPlaneList.end() - 2;
+         plane[0].set(p3,p2,p1);
+         plane[1] = plane[0];
+         plane[1].invert();
+
+         U32 pmask = U32leftShift[mPlaneList.size()-2]; // U64(1) << (mPlaneList.size()-2)
+         ef1.planeMask |= pmask;
+         ef2.planeMask |= pmask << 1;
+      }
+   }
+}
+
+
+//----------------------------------------------------------------------------
+
+void ExtrudedPolyList::setCollisionList(CollisionList* info)
+{
+   mCollisionList = info;
+   mCollisionList->count = 0;
+   mCollisionList->t = 2;
+}
+
+
+//----------------------------------------------------------------------------
+
+void ExtrudedPolyList::adjustCollisionTime()
+{
+   // Called after all the polys have been added.
+   // There was a reason for doing it here instead of subtracting
+   // the face Epsilon (faceShift) when the closest point is calculated,
+   // but I can't remember what it is...
+   if (mCollisionList->count) {
+      mCollisionList->t -= mFaceShift;
+      if (mCollisionList->t > 1)
+         mCollisionList->t = 1;
+      else
+         if (mCollisionList->t < 0)
+            mCollisionList->t = 0;
+   }
+}
+
+
+//----------------------------------------------------------------------------
+
+U32 ExtrudedPolyList::addPoint(const Point3F& p)
+{
+   mVertexList.increment();
+   Vertex& v = mVertexList.last();
+
+   v.point.x = p.x * mScale.x;
+   v.point.y = p.y * mScale.y;
+   v.point.z = p.z * mScale.z;
+   mMatrix.mulP(v.point);
+
+   // Build the plane mask, planes come in pairs
+   v.mask = 0;
+   for (U32 i = 0; i < mPlaneList.size(); i += 2)
+   {
+      F32 dist = mPlaneList[i].distToPlane(v.point);
+      if (dist >= sgFrontEpsilon)
+         v.mask |= U32leftShift[i];   // U64(1) << i;
+      else
+         v.mask |= U32leftShift[i+1]; // U64(2) << i;
+   }
+   return mVertexList.size() - 1;
+}
+
+
+U32 ExtrudedPolyList::addPlane(const PlaneF& plane)
+{
+   mPolyPlaneList.increment();
+   mPlaneTransformer.transform(plane, mPolyPlaneList.last());
+
+   return mPolyPlaneList.size() - 1;
+}
+
+
+//----------------------------------------------------------------------------
+
+void ExtrudedPolyList::begin(U32 material, U32 /*surfaceKey*/)
+{
+   mPoly.object = mCurrObject;
+   mPoly.material = material;
+   mIndexList.clear();
+}
+
+void ExtrudedPolyList::plane(U32 v1, U32 v2, U32 v3)
+{
+   mPoly.plane.set(mVertexList[v1].point,
+                   mVertexList[v2].point,
+                   mVertexList[v3].point);
+}
+
+void ExtrudedPolyList::plane(const PlaneF& p)
+{
+   mPlaneTransformer.transform(p, mPoly.plane);
+}
+
+void ExtrudedPolyList::plane(const U32 index)
+{
+   AssertFatal(index < mPolyPlaneList.size(), "Out of bounds index!");
+   mPoly.plane = mPolyPlaneList[index];
+}
+
+const PlaneF& ExtrudedPolyList::getIndexedPlane(const U32 index)
+{
+   AssertFatal(index < mPolyPlaneList.size(), "Out of bounds index!");
+   return mPolyPlaneList[index];
+}
+
+
+void ExtrudedPolyList::vertex(U32 vi)
+{
+   mIndexList.push_back(vi);
+}
+
+void ExtrudedPolyList::end()
+{
+   // Anything facing away from the mVelocity is rejected
+   if (mCollisionList->count >= CollisionList::MaxCollisions ||
+       mDot(mPoly.plane, mNormalVelocity) > 0)
+      return;
+
+   // Test the mPoly against the planes each extruded face.
+   U32 cFaceCount = 0;
+   ExtrudedFace* cFace[30];
+   bool          cEdgeColl[30];
+   ExtrudedFace* face = mExtrudedList.begin();
+   ExtrudedFace* end = mExtrudedList.end();
+   for (; face != end; face++)
+   {
+      if (face->active && (face->faceDot = -mDot(face->plane,mPoly.plane)) > 0)
+      {
+         if (testPoly(*face)) {
+            cFace[cFaceCount]       = face;
+            cEdgeColl[cFaceCount++] = false;
+         }
+      }
+   }
+   if (!cFaceCount)
+   {
+      face = mExtrudedList.begin();
+      end  = mExtrudedList.end();
+      for (; face != end; face++)
+      {
+         if (face->active && (face->faceDot = -mDot(face->plane,mPoly.plane)) <= 0)
+         {
+            if (testPoly(*face)) {
+               cFace[cFaceCount]       = face;
+               cEdgeColl[cFaceCount++] = true;
+            }
+         }
+      }
+   }
+   if (!cFaceCount)
+      return;
+
+   // Pick the best collision face
+   face = cFace[0];
+   bool edge = cEdgeColl[0];
+   for (U32 f = 1; f < cFaceCount; f++)
+   {
+      if (cFace[f]->faceDot > face->faceDot)
+      {
+         face = cFace[f];
+         edge = cEdgeColl[f];
+      }
+   }
+   // Add it to the collision list if it's better and/or equal
+   // to our current best.
+   if (face->time <= mCollisionList->t + EqualEpsilon) {
+      if (face->time < mCollisionList->t - EqualEpsilon) {
+         mFaceShift = face->faceShift;
+         mCollisionList->t = face->time;
+         mCollisionList->count = 0;
+         mCollisionList->maxHeight = face->height;
+      }
+      else {
+         if (face->height > mCollisionList->maxHeight)
+            mCollisionList->maxHeight = face->height;
+         if (face->faceShift > mFaceShift)
+            mFaceShift = face->faceShift;
+      }
+      Collision& collision = mCollisionList->collision[mCollisionList->count++];
+      collision.point    = face->point;
+      collision.faceDot  = face->faceDot;
+      collision.face     = face - mExtrudedList.begin();
+      collision.object   = mPoly.object;
+      collision.normal   = mPoly.plane;
+      collision.material = mPoly.material;
+   }
+}
+
+
+//----------------------------------------------------------------------------
+
+bool ExtrudedPolyList::testPoly(ExtrudedFace& face)
+{
+   // Build intial inside/outside plane masks
+   U32 indexStart = 0;
+   U32 indexEnd = mIndexList.size();
+   U32 oVertexSize = mVertexList.size();
+   U32 oIndexSize = mIndexList.size();
+
+   U32 frontMask = 0,backMask = 0;
+   for (U32 i = indexStart; i < indexEnd; i++) {
+      U32 mask = mVertexList[mIndexList[i]].mask & face.planeMask;
+      frontMask |= mask;
+      backMask |= ~mask;
+   }
+
+   // Clip the mPoly against the planes that bound the face...
+   // Trivial accept if all the vertices are on the backsides of
+   // all the planes.
+   if (frontMask) {
+      // Trivial reject if any plane not crossed has all it's points
+      // on the front.
+      U32 crossMask = frontMask & backMask;
+      if (~crossMask & frontMask)
+         return false;
+
+      // Need to do some clipping
+      for (U32 p = 0; p < mPlaneList.size(); p++) {
+         U32 pmask = U32leftShift[p]; // U64(1) << p;
+         U32 newStart = mIndexList.size();
+
+         // Only test against this plane if we have something
+         // on both sides
+         if (face.planeMask & crossMask & pmask) {
+            U32 i1 = indexEnd - 1;
+            U32 mask1 = mVertexList[mIndexList[i1]].mask;
+
+            for (U32 i2 = indexStart; i2 < indexEnd; i2++) {
+               U32 mask2 = mVertexList[mIndexList[i2]].mask;
+               if ((mask1 ^ mask2) & pmask) {
+                  //
+                  mVertexList.increment();
+                  VectorF& v1 = mVertexList[mIndexList[i1]].point;
+                  VectorF& v2 = mVertexList[mIndexList[i2]].point;
+                  VectorF vv = v2 - v1;
+                  F32 t = -mPlaneList[p].distToPlane(v1) / mDot(mPlaneList[p],vv);
+
+                  mIndexList.push_back(mVertexList.size() - 1);
+                  Vertex& iv = mVertexList.last();
+                  iv.point.x = v1.x + vv.x * t;
+                  iv.point.y = v1.y + vv.y * t;
+                  iv.point.z = v1.z + vv.z * t;
+                  iv.mask = 0;
+
+                  // Test against the remaining planes
+                  for (U32 i = p + 1; i < mPlaneList.size(); i++) {
+                     U32 mask = U32leftShift[i]; // U64(1) << i;
+                     if (face.planeMask & mask &&
+                           mPlaneList[i].distToPlane(iv.point) > 0) {
+                        iv.mask = mask;
+                        break;
+                     }
+                  }
+               }
+               if (!(mask2 & pmask)) {
+                  U32 index = mIndexList[i2];
+                  mIndexList.push_back(index);
+               }
+               mask1 = mask2;
+               i1 = i2;
+            }
+
+            // Check for degenerate
+            indexStart = newStart;
+            indexEnd = mIndexList.size();
+            if (mIndexList.size() - indexStart < 3) {
+               mVertexList.setSize(oVertexSize);
+               mIndexList.setSize(oIndexSize);
+               return false;
+            }
+         }
+      }
+   }
+
+   // Find closest point on the mPoly
+   Point3F bp;
+   F32 bd = 1E30;
+   F32 height = -1E30;
+   for (U32 b = indexStart; b < indexEnd; b++) {
+      Vertex& vertex = mVertexList[mIndexList[b]];
+      F32 dist = face.plane.distToPlane(vertex.point);
+      if (dist <= bd) {
+         bd = (dist < 0)? 0: dist;
+         bp = vertex.point;
+      }
+      // Since we don't clip against the back plane, we'll
+      // only include vertex heights that are within range.
+      if (vertex.point.z > height && dist < face.maxDistance)
+         height = vertex.point.z;
+   }
+
+   // Remove temporary data
+   mVertexList.setSize(oVertexSize);
+   mIndexList.setSize(oIndexSize);
+
+   // Add it to the collision list if it's better and/or equal
+   // to our current best.
+   if (bd < face.maxDistance + FaceEpsilon) {
+      face.time = bd / face.maxDistance;
+      face.height = height;
+      face.point = bp;
+      return true;
+   }
+   return false;
+}
+
+
+//----------------------------------------------------------------------------
+
+void ExtrudedPolyList::render()
+{
+   if (!mCollisionList)
+      return;
+
+   glBegin(GL_LINES);
+   glColor3f(1,1,0);
+
+   for (U32 d = 0; d < mCollisionList->count; d++) {
+      Collision& face = mCollisionList->collision[d];
+      Point3F ep = face.point;
+      ep += face.normal;
+      glVertex3fv(face.point);
+      glVertex3fv(ep);
+   }
+
+   glEnd();
+}
+
+//--------------------------------------------------------------------------
+void ExtrudedPolyList::setVelocity(const VectorF& velocity)
+{
+   mVelocity = velocity;
+   if (velocity.isZero() == false)
+   {
+      mNormalVelocity = velocity;
+      mNormalVelocity.normalize();
+      setInterestNormal(mNormalVelocity);
+   }
+   else
+   {
+      mNormalVelocity.set(0, 0, 0);
+      clearInterestNormal();
+   }
+}
+
+