Add capsule/capsule and capsule/sphere collision algorithm

CMakeLists.txt

@@ -78,6 +78,8 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
     "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
     "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
+    "src/collision/narrowphase/SphereVsCapsuleAlgorithm.h"
+    "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
     "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
     "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
     "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -30,8 +30,122 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
 
-bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
-                                            ContactPointInfo& contactPointInfo) {
+bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
     
+	const decimal parallelEpsilon = decimal(0.001);
 
+    // Get the capsule collision shapes
+    const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
+    const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
+
+	// Get the transform from capsule 1 local-space to capsule 2 local-space
+	const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
+
+	// Compute the end-points of the inner segment of the first capsule
+	Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
+	Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
+	capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
+	capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
+
+	// Compute the end-points of the inner segment of the second capsule
+	const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
+	const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
+	
+	// The two inner capsule segments
+	const Vector3 seg1 = capsule1SegB - capsule1SegA;
+	const Vector3 seg2 = capsule2SegB - capsule2SegA;
+
+	// Compute the sum of the radius of the two capsules (virtual spheres)
+	decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
+
+	// If the two capsules are parallel (we create two contact points)
+	if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) {
+
+		// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
+		const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
+		if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) {
+
+			// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
+			// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
+			return false;
+		}
+
+		// Compute the planes that goes through the extrem points of the inner segment of capsule 1
+		decimal d1 = seg1.dot(capsule1SegA);
+		decimal d2 = -seg1.dot(capsule1SegB);
+
+		// Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
+		// segment of capsule 1
+		decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
+		decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
+
+		bool isClipValid = false;	// True if the segments were overlapping (the clip segment is valid)
+
+		// Clip the inner segment of capsule 2
+		Vector3 clipPointA, clipPointB;
+		if (t1 >= decimal(0.0)) {
+
+			if (t1 > decimal(1.0)) t1 = decimal(1.0);
+			clipPointA = capsule2SegB - t1 * seg2;
+			isClipValid = true;
+		}
+		if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
+
+			if (t2 > decimal(1.0)) t2 = decimal(1.0);
+			clipPointB = capsule2SegA + t2 * seg2;
+			isClipValid = true;
+		}
+
+		// If we have a valid clip segment
+		if (isClipValid) {
+
+			Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA);
+			Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
+
+			const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+			const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+			const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+			const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+
+			const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
+
+			decimal penetrationDepth = sumRadius - segmentsDistance;
+
+			// Create the contact info object
+			// TODO : Make sure we create two contact points at the same time (same method here)
+			contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local);
+			contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local);
+		}
+	}
+
+	// Compute the closest points between the two inner capsule segments
+	Vector3 closestPointCapsule1Seg;
+	Vector3 closestPointCapsule2Seg;
+	computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
+		closestPointCapsule1Seg, closestPointCapsule2Seg);
+
+	// Compute the distance between the sphere center and the closest point on the segment
+	Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
+	const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
+	
+	// If the collision shapes overlap
+	if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
+
+		decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
+		closestPointsSeg1ToSeg2 /= closestPointsDistance;
+
+		const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
+		const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
+
+		const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
+
+		decimal penetrationDepth = sumRadius - closestPointsDistance;
+
+		// Create the contact info object
+		contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+
+		return true;
+	}
+
+	return false;
 }

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h

@@ -38,7 +38,7 @@ namespace reactphysics3d {
 // Class CapsuleVsCapsuleAlgorithm
 /**
  * This class is used to compute the narrow-phase collision detection
- * between two capsule collision shapes.
+ * between two capsules collision shapes.
  */
 class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 

src/mathematics/mathematics_functions.cpp

@@ -26,6 +26,7 @@
 // Libraries
 #include "mathematics_functions.h"
 #include "Vector3.h"
+#include <cassert>
 
 using namespace reactphysics3d;
 
@@ -50,10 +51,152 @@ void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, c
     u = decimal(1.0) - v - w;
 }
 
-// Clamp a vector such that it is no longer than a given maximum length
+/// Clamp a vector such that it is no longer than a given maximum length
 Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
     if (vector.lengthSquare() > maxLength * maxLength) {
         return vector.getUnit() * maxLength;
     }
     return vector;
 }
+
+/// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
+Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) {
+
+	const Vector3 ab = segPointB - segPointA;
+
+	decimal abLengthSquare = ab.lengthSquare();
+
+	// If the segment has almost zero length
+	if (abLengthSquare < MACHINE_EPSILON) {
+
+		// Return one end-point of the segment as the closest point
+		return segPointA;
+	}
+
+	// Project point C onto "AB" line
+	decimal t = (pointC - segPointA).dot(ab) / abLengthSquare;
+
+	// If projected point onto the line is outside the segment, clamp it to the segment
+	if (t < decimal(0.0)) t = decimal(0.0);
+	if (t > decimal(1.0)) t = decimal(1.0);
+
+	// Return the closest point on the segment
+	return segPointA + t * ab;
+}
+
+/// Compute the closest points between two segments
+/// This method uses the technique described in the book Real-Time
+/// collision detection by Christer Ericson.
+void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
+										   const Vector3& seg2PointA, const Vector3& seg2PointB,
+										   Vector3& closestPointSeg1, Vector3& closestPointSeg2) {
+
+	const Vector3 d1 = seg1PointB - seg1PointA;
+	const Vector3 d2 = seg2PointB - seg2PointA;
+	const Vector3 r = seg1PointA - seg2PointA;
+	decimal a = d1.lengthSquare();
+	decimal e = d2.lengthSquare();
+	decimal f = d2.dot(r);
+	decimal s, t;
+
+	// If both segments degenerate into points
+	if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) {
+
+		closestPointSeg1 = seg1PointA;
+		closestPointSeg2 = seg2PointA;
+		return;
+	}
+	if (a <= MACHINE_EPSILON) {   // If first segment degenerates into a point
+		
+		s = decimal(0.0);
+
+		// Compute the closest point on second segment
+		t = clamp(f / e, decimal(0.0), decimal(1.0));
+	}
+	else {
+
+		decimal c = d1.dot(r);
+
+		// If the second segment degenerates into a point
+		if (e <= MACHINE_EPSILON) {
+
+			t = decimal(0.0);
+			s = clamp(-c / a, decimal(0.0), decimal(1.0));
+		}
+		else {
+
+			decimal b = d1.dot(d2);
+			decimal denom = a * e - b * b;
+
+			// If the segments are not parallel
+			if (denom != decimal(0.0)) {
+
+				// Compute the closest point on line 1 to line 2 and
+				// clamp to first segment.
+				s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0));
+			}
+			else {
+
+				// Pick an arbitrary point on first segment
+				s = decimal(0.0);
+			}
+
+			// Compute the point on line 2 closest to the closest point
+			// we have just found
+			t = (b * s + f) / e;
+
+			// If this closest point is inside second segment (t in [0, 1]), we are done.
+			// Otherwise, we clamp the point to the second segment and compute again the
+			// closest point on segment 1
+			if (t < decimal(0.0)) {
+				t = decimal(0.0);
+				s = clamp(-c / a, decimal(0.0), decimal(1.0));
+			}
+			else if (t > decimal(1.0)) {
+				t = decimal(1.0);
+				s = clamp((b - c) / a, decimal(0.0), decimal(1.0));
+			}
+		}
+	}
+
+	// Compute the closest points on both segments
+	closestPointSeg1 = seg1PointA + d1 * s;
+	closestPointSeg2 = seg2PointA + d2 * t;
+}
+
+/// Compute the intersection between a plane and a segment
+// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method
+// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such
+// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise,
+// there is no intersection between the plane and the segment.
+decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) {
+
+	const decimal parallelEpsilon = decimal(0.0001);
+	decimal t = decimal(-1);
+
+	// Segment AB
+	const Vector3 ab = segB - segA;
+
+	decimal nDotAB = planeNormal.dot(ab);
+
+	// If the segment is not parallel to the plane
+	if (nDotAB > parallelEpsilon) {
+		t = (planeD - planeNormal.dot(segA)) / nDotAB;
+	}
+
+	return t;
+}
+
+/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
+decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
+	
+	decimal distAB = (linePointB - linePointA).length();
+
+	if (distAB < MACHINE_EPSILON) {
+		return (point - linePointA).length();
+	}
+
+	return ((point - linePointA).cross(point - linePointB)).length() / distAB;
+}
+
+

src/mathematics/mathematics_functions.h

@@ -78,10 +78,26 @@ inline bool sameSign(decimal a, decimal b) {
 /// Clamp a vector such that it is no longer than a given maximum length
 Vector3 clamp(const Vector3& vector, decimal maxLength);
 
+// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
+Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC);
+
+// Compute the closest points between two segments
+void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
+										   const Vector3& seg2PointA, const Vector3& seg2PointB,
+										   Vector3& closestPointSeg1, Vector3& closestPointSeg2);
+
 /// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
 void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
                                              const Vector3& p, decimal& u, decimal& v, decimal& w);
 
+/// Compute the intersection between a plane and a segment
+decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal);
+
+/// Compute the distance between a point and a line
+decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
+
 }
 
+
+
 #endif

test/tests/mathematics/TestMathematicsFunctions.h

@@ -124,6 +124,52 @@ class TestMathematicsFunctions : public Test {
 
             computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w);
             test(approxEqual(u + v + w, 1.0, 0.000001));
+
+			// Test computeClosestPointBetweenTwoSegments()
+			Vector3 closestSeg1, closestSeg2;
+			computeClosestPointBetweenTwoSegments(Vector3(4, 0, 0), Vector3(6, 0, 0), Vector3(8, 0, 0), Vector3(8, 6, 0), closestSeg1, closestSeg2);
+			test(approxEqual(closestSeg1.x, 6.0, 0.000001));
+			test(approxEqual(closestSeg1.y, 0.0, 0.000001));
+			test(approxEqual(closestSeg1.z, 0.0, 0.000001));
+			test(approxEqual(closestSeg2.x, 8.0, 0.000001));
+			test(approxEqual(closestSeg2.y, 0.0, 0.000001));
+			test(approxEqual(closestSeg2.z, 0.0, 0.000001));
+			computeClosestPointBetweenTwoSegments(Vector3(4, 6, 5), Vector3(4, 6, 5), Vector3(8, 3, -9), Vector3(8, 3, -9), closestSeg1, closestSeg2);
+			test(approxEqual(closestSeg1.x, 4.0, 0.000001));
+			test(approxEqual(closestSeg1.y, 6.0, 0.000001));
+			test(approxEqual(closestSeg1.z, 5.0, 0.000001));
+			test(approxEqual(closestSeg2.x, 8.0, 0.000001));
+			test(approxEqual(closestSeg2.y, 3.0, 0.000001));
+			test(approxEqual(closestSeg2.z, -9.0, 0.000001));
+			computeClosestPointBetweenTwoSegments(Vector3(0, -5, 0), Vector3(0, 8, 0), Vector3(6, 3, 0), Vector3(10, -3, 0), closestSeg1, closestSeg2);
+			test(approxEqual(closestSeg1.x, 0.0, 0.000001));
+			test(approxEqual(closestSeg1.y, 3.0, 0.000001));
+			test(approxEqual(closestSeg1.z, 0.0, 0.000001));
+			test(approxEqual(closestSeg2.x, 6.0, 0.000001));
+			test(approxEqual(closestSeg2.y, 3.0, 0.000001));
+			test(approxEqual(closestSeg2.z, 0.0, 0.000001));
+			computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2);
+			test(approxEqual(closestSeg1.x, 1.0, 0.000001));
+			test(approxEqual(closestSeg1.y, 5.0, 0.000001));
+			test(approxEqual(closestSeg1.z, -5.0, 0.000001));
+			test(approxEqual(closestSeg2.x, 1.0, 0.000001));
+			test(approxEqual(closestSeg2.y, 5.0, 0.000001));
+			test(approxEqual(closestSeg2.z, -5.0, 0.000001));
+
+			// Test computePlaneSegmentIntersection();
+			test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001));
+			test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001));
+			test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001));
+			test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001));
+			decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0));
+			test(tIntersect < 0.0 && tIntersect > 1.0);
+
+			// Test computeDistancePointToLineDistance()
+			test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001));
+			test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001));
+			test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001));
+			test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001));
+			test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001));
         }
 
  };

testbed/scenes/collisiondetection/CollisionDetectionScene.cpp

@@ -71,6 +71,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
     mSphere2->setColor(mGreyColorDemo);
     mSphere2->setSleepingColor(mRedColorDemo);
 
+	// ---------- Capsule 1 ---------- //
+	openglframework::Vector3 position3(4, 0, 0);
+
+	// Create a cylinder and a corresponding collision body in the dynamics world
+	mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
+	mAllShapes.push_back(mCapsule1);
+
+	// Set the color
+	mCapsule1->setColor(mGreyColorDemo);
+	mCapsule1->setSleepingColor(mRedColorDemo);
+
     // ---------- Cone ---------- //
     //openglframework::Vector3 position4(0, 0, 0);
 
@@ -93,16 +104,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
     //mCylinder->setColor(mGreyColorDemo);
     //mCylinder->setSleepingColor(mRedColorDemo);
 
-    // ---------- Capsule ---------- //
-    //openglframework::Vector3 position6(0, 0, 0);
-
-    // Create a cylinder and a corresponding collision body in the dynamics world
-    //mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 ,
-    //                       mCollisionWorld, mMeshFolderPath);
-
-    // Set the color
-    //mCapsule->setColor(mGreyColorDemo);
-    //mCapsule->setSleepingColor(mRedColorDemo);
 
     // ---------- Convex Mesh ---------- //
     //openglframework::Vector3 position7(0, 0, 0);
@@ -135,7 +136,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
     //mHeightField->setSleepingColor(mRedColorDemo);
 
     // Create the VBO and VAO to render the lines
-    createVBOAndVAO(mPhongShader);
+    //createVBOAndVAO(mPhongShader);
 
     mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
 }
@@ -283,6 +284,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
     // Render the shapes
     if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
     if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+	if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
 
     /*
     if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);

testbed/scenes/collisiondetection/CollisionDetectionScene.h

@@ -33,6 +33,8 @@
 #include "SceneDemo.h"
 #include "Sphere.h"
 #include "Box.h"
+#include "Cone.h"
+#include "Cylinder.h"
 #include "Capsule.h"
 #include "Line.h"
 #include "ConvexMesh.h"
@@ -136,6 +138,8 @@ class CollisionDetectionScene : public SceneDemo {
         //Box* mBox;
         Sphere* mSphere1;
         Sphere* mSphere2;
+		Capsule* mCapsule1;
+		Capsule* mCapsule2;
         //Cone* mCone;
         //Cylinder* mCylinder;
         //Capsule* mCapsule;