Merge pull request #1290 from marauder2k9-torque/PhysicsDiscovery-tests

Plane Convex

Engine/source/T3D/groundPlane.cpp

@@ -272,37 +272,35 @@ void GroundPlane::buildConvex( const Box3F& box, Convex* convex )
       return;
 
    // See if we already have a convex in the working set.
-   BoxConvex *boxConvex = NULL;
+   PlaneConvex *planeConvex = NULL;
    CollisionWorkingList &wl = convex->getWorkingList();
    CollisionWorkingList *itr = wl.wLink.mNext;
    for ( ; itr != &wl; itr = itr->wLink.mNext )
    {
-      if (  itr->mConvex->getType() == BoxConvexType &&
+      if (  itr->mConvex->getType() == PlaneConvexType &&
             itr->mConvex->getObject() == this )
       {
-         boxConvex = (BoxConvex*)itr->mConvex;
+         planeConvex = (PlaneConvex*)itr->mConvex;
          break;
       }
    }
 
-   if ( !boxConvex )
+   if ( !planeConvex)
    {
-      boxConvex = new BoxConvex;
+      planeConvex = new PlaneConvex;
-      mConvexList->registerObject( boxConvex );
+      mConvexList->registerObject(planeConvex);
-      boxConvex->init( this );
+      planeConvex->init( this );
 
-      convex->addToWorkingList( boxConvex );
+      convex->addToWorkingList(planeConvex);
    }
 
    // Update our convex to best match the queried box
-   if ( boxConvex )
+   if (planeConvex)
    {
       Point3F queryCenter = box.getCenter();
 
-      boxConvex->mCenter      = Point3F( queryCenter.x, queryCenter.y, -GROUND_PLANE_BOX_HEIGHT_HALF );
+      planeConvex->mCenter = Point3F( queryCenter.x, queryCenter.y, 0 );
-      boxConvex->mSize        = Point3F( box.getExtents().x,
+      planeConvex->mSize   = Point3F( box.getExtents().x, box.getExtents().y, 0 );
-                                         box.getExtents().y,
-                                         GROUND_PLANE_BOX_HEIGHT_HALF );
    }
 }
 

Engine/source/collision/boxConvex.cpp

@@ -215,3 +215,100 @@ const MatrixF& OrthoBoxConvex::getTransform() const
    return mOrthoMatrixCache;
 }
 
+Point3F PlaneConvex::getVertex(S32 v)
+{
+   Point3F p = mCenter;
+   p.x += (v & 1) ? mSize.x : -mSize.x;
+   p.y += (v & 2) ? mSize.y : -mSize.y;
+   
+   return p;
+}
+
+void PlaneConvex::emitEdge(S32 v1, S32 v2, const MatrixF& mat, ConvexFeature* cf)
+{
+   S32 vc = cf->mVertexList.size();
+   cf->mVertexList.increment(2);
+   Point3F* vp = cf->mVertexList.begin();
+   mat.mulP(getVertex(v1), &vp[vc]);
+   mat.mulP(getVertex(v2), &vp[vc + 1]);
+
+   cf->mEdgeList.increment();
+   ConvexFeature::Edge& edge = cf->mEdgeList.last();
+   edge.vertex[0] = vc;
+   edge.vertex[1] = vc + 1;
+}
+
+void PlaneConvex::emitFace(const MatrixF& mat, ConvexFeature* cf) {
+   // Assuming sFace contains a single face definition for the plane
+   Face& face = sFace[4];
+
+   // Emit vertices
+   S32 vc = cf->mVertexList.size();
+   cf->mVertexList.increment(4);
+   Point3F* vp = cf->mVertexList.begin();
+   for (S32 v = 0; v < 4; v++) {
+      mat.mulP(getVertex(face.vertex[v]), &vp[vc + v]);
+   }
+
+   // Emit edges
+   cf->mEdgeList.increment(4);
+   ConvexFeature::Edge* edge = cf->mEdgeList.end() - 4;
+   for (S32 e = 0; e < 4; e++) {
+      edge[e].vertex[0] = vc + e;
+      edge[e].vertex[1] = vc + ((e + 1) & 3);
+   }
+
+   // Emit 2 triangle faces
+   cf->mFaceList.increment(2);
+   ConvexFeature::Face* ef = cf->mFaceList.end() - 2;
+   mat.getColumn(face.axis, &ef->normal);
+   ef[1].normal = ef[0].normal;
+   ef[1].vertex[0] = ef[0].vertex[0] = vc;
+   ef[1].vertex[1] = ef[0].vertex[2] = vc + 2;
+   ef[0].vertex[1] = vc + 1;
+   ef[1].vertex[2] = vc + 3;
+}
+
+Point3F PlaneConvex::support(const VectorF& v) const {
+   Point3F p = mCenter;
+   p.x += (v.x >= 0) ? mSize.x : -mSize.x;
+   p.y += (v.y >= 0) ? mSize.y : -mSize.y;
+   return p;
+}
+
+void PlaneConvex::getFeatures(const MatrixF& mat, const VectorF& n, ConvexFeature* cf)
+{
+   cf->material = 0;
+   cf->mObject = mObject;
+
+   // Emit edges
+   for (S32 i = 0; i < 4; ++i) {
+      S32 next = (i + 1) % 4;
+      emitEdge(i, next, mat, cf);
+   }
+
+   emitFace(mat, cf);
+}
+
+void PlaneConvex::getPolyList(AbstractPolyList* list)
+{
+   list->setTransform(&getTransform(), getScale());
+   list->setObject(getObject());
+
+   U32 base =  list->addPoint(mCenter + Point3F(-mSize.x, -mSize.y, -mSize.z));
+               list->addPoint(mCenter + Point3F(mSize.x, -mSize.y, -mSize.z));
+               list->addPoint(mCenter + Point3F(-mSize.x, mSize.y, -mSize.z));
+               list->addPoint(mCenter + Point3F(mSize.x, mSize.y, -mSize.z));
+
+   list->begin(0, 0);
+
+   list->vertex(base + sFace[4].vertex[3]);
+   list->vertex(base + sFace[4].vertex[2]);
+   list->vertex(base + sFace[4].vertex[1]);
+   list->vertex(base + sFace[4].vertex[0]);
+
+   list->plane(base + sFace[4].vertex[2],
+               base + sFace[4].vertex[1],
+               base + sFace[4].vertex[0]);
+   list->end();
+}

Engine/source/collision/boxConvex.h

@@ -48,16 +48,32 @@ public:
    void getPolyList(AbstractPolyList* list) override;
 };
 
-
+class OrthoBoxConvex : public BoxConvex
-class OrthoBoxConvex: public BoxConvex
 {
    typedef BoxConvex Parent;
    mutable MatrixF mOrthoMatrixCache;
 
- public:
+public:
    OrthoBoxConvex() { mOrthoMatrixCache.identity(); }
 
    const MatrixF& getTransform() const override;
 };
 
+class PlaneConvex : public Convex
+{
+   Point3F getVertex(S32 v);
+   void emitEdge(S32 v1, S32 v2, const MatrixF& mat, ConvexFeature* cf);
+   void emitFace(const MatrixF& mat, ConvexFeature* cf);
+public:
+   Point3F mCenter;
+   VectorF mSize;
+
+   PlaneConvex() { mType = PlaneConvexType; }
+   void init(SceneObject* obj) { mObject = obj; }
+
+   Point3F support(const VectorF& v) const override;
+   void getFeatures(const MatrixF& mat, const VectorF& n, ConvexFeature* cf) override;
+   void getPolyList(AbstractPolyList* list) override;
+};
+
 #endif

Engine/source/collision/concretePolyList.cpp

@@ -150,13 +150,13 @@ void ConcretePolyList::render()
    GFXStateBlockRef sb = GFX->createStateBlock( solidZDisable );
    GFX->setStateBlock( sb );
 
-   PrimBuild::color3i( 255, 0, 255 );
-
    Poly *p;
    Point3F *pnt;
 
    for ( p = mPolyList.begin(); p < mPolyList.end(); p++ )
    {
+      PrimBuild::color3i(255, 0, 255);
+
       PrimBuild::begin( GFXLineStrip, p->vertexCount + 1 );      
 
       for ( U32 i = 0; i < p->vertexCount; i++ )
@@ -169,6 +169,31 @@ void ConcretePolyList::render()
       PrimBuild::vertex3fv( pnt );
 
       PrimBuild::end();
+
+      // Calculate the center of the polygon
+      Point3F centroid(0, 0, 0);
+      for (U32 i = 0; i < p->vertexCount; i++)
+      {
+         pnt = &mVertexList[mIndexList[p->vertexStart + i]];
+         centroid += *pnt;
+      }
+      centroid /= p->vertexCount;
+
+      // Calculate the end point of the normal line
+      Point3F norm = p->plane.getNormal();
+
+      U8 red = static_cast<U8>((norm.x + 1.0f) * 0.5f * 255);
+      U8 green = static_cast<U8>((norm.y + 1.0f) * 0.5f * 255);
+      U8 blue = static_cast<U8>((norm.z + 1.0f) * 0.5f * 255);
+
+      PrimBuild::color3i(red, green, blue);
+      Point3F normalEnd = centroid + norm;
+
+      // Draw the normal line
+      PrimBuild::begin(GFXLineList, 2);
+      PrimBuild::vertex3fv(centroid);
+      PrimBuild::vertex3fv(normalEnd);
+      PrimBuild::end();
    }   
 }
 
@@ -220,4 +245,4 @@ void ConcretePolyList::triangulate()
 
    mPolyList = polyList;
    mIndexList = indexList;
-}
+}

Engine/source/collision/convex.h

@@ -90,7 +90,8 @@ enum ConvexType {
    TSPolysoupConvexType,
    MeshRoadConvexType,
 	ConvexShapeCollisionConvexType,
-   ForestConvexType
+   ForestConvexType,
+   PlaneConvexType
 };