diff --git a/Simulations/RigidBodySystemSimulator.h b/Simulations/RigidBodySystemSimulator.h new file mode 100644 index 0000000..4ba678e --- /dev/null +++ b/Simulations/RigidBodySystemSimulator.h @@ -0,0 +1,46 @@ +#ifndef RIGIDBODYSYSTEMSIMULATOR_h +#define RIGIDBODYSYSTEMSIMULATOR_h +#include "Simulator.h" +//add your header for your rigid body system, for e.g., +//#include "rigidBodySystem.h" + +#define TESTCASEUSEDTORUNTEST 2 + +class RigidBodySystemSimulator:public Simulator{ +public: + // Construtors + RigidBodySystemSimulator(); + + // Functions + const char * getTestCasesStr(); + void initUI(DrawingUtilitiesClass * DUC); + void reset(); + void drawFrame(ID3D11DeviceContext* pd3dImmediateContext); + void notifyCaseChanged(int testCase); + void externalForcesCalculations(float timeElapsed); + void simulateTimestep(float timeStep); + void onClick(int x, int y); + void onMouse(int x, int y); + + // ExtraFunctions + int getNumberOfRigidBodies(); + Vec3 getPositionOfRigidBody(int i); + Vec3 getLinearVelocityOfRigidBody(int i); + Vec3 getAngularVelocityOfRigidBody(int i); + void applyForceOnBody(int i, Vec3 loc, Vec3 force); + void addRigidBody(Vec3 position, Vec3 size, int mass); + void setOrientationOf(int i,Quat orientation); + void setVelocityOf(int i, Vec3 velocity); + +private: + // Attributes + // add your RigidBodySystem data members, for e.g., + // RigidBodySystem * m_pRigidBodySystem; + Vec3 m_externalForce; + + // UI Attributes + Point2D m_mouse; + Point2D m_trackmouse; + Point2D m_oldtrackmouse; + }; +#endif \ No newline at end of file diff --git a/Simulations/collisionDetect.h b/Simulations/collisionDetect.h new file mode 100644 index 0000000..ea04d87 --- /dev/null +++ b/Simulations/collisionDetect.h @@ -0,0 +1,477 @@ +// header file: +#include +#include +using namespace DirectX; + +// the return structure, with these values, you should be able to calculate the impulse +// the depth shouldn't be used in your impulse calculation, it is a redundant value +// if the normalWorld == XMVectorZero(), no collision +struct CollisionInfo{ + bool isValid; // whether there is a collision point, true for yes + GamePhysics::Vec3 collisionPointWorld; // the position of the collision point in world space + GamePhysics::Vec3 normalWorld; // the direction of the impulse to A, negative of the collision face of A + float depth; // the distance of the collision point to the surface, not necessary. +}; + +// tool data structures/functions called by the collision detection method, you can ignore the details here +namespace collisionTools{ + struct Projection{ + float min, max; + }; + + + inline std::vector discritizeObject(const XMMATRIX& obj2World) + { + const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World); + XMVECTOR edges[3]; + std::vector results; + for (int precession = 0.1; precession <= 0.5; precession += 0.1) + { + for (size_t i = 0; i < 3; ++i) + edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), precession, i), obj2World); + results.push_back(centerWorld - edges[0] - edges[1] - edges[2]); + results.push_back(centerWorld + edges[0] - edges[1] - edges[2]); + results.push_back(centerWorld - edges[0] + edges[1] - edges[2]); + results.push_back(centerWorld + edges[0] + edges[1] - edges[2]); + results.push_back(centerWorld - edges[0] - edges[1] + edges[2]); + results.push_back(centerWorld + edges[0] - edges[1] + edges[2]); + results.push_back(centerWorld - edges[0] + edges[1] + edges[2]); + results.push_back(centerWorld + edges[0] + edges[1] + edges[2]); + } + } + + inline XMVECTOR getVectorConnnectingCenters(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B) + { + const XMVECTOR centerWorld_A = XMVector3Transform(XMVectorZero(), obj2World_A); + const XMVECTOR centerWorld_B = XMVector3Transform(XMVectorZero(), obj2World_B); + return centerWorld_B - centerWorld_A; + + } + + // Get Corners + inline std::vector getCorners(const XMMATRIX& obj2World) + { + const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World); + XMVECTOR edges[3]; + for (size_t i = 0; i < 3; ++i) + edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), 0.5f, i), obj2World); + std::vector results; + results.push_back(centerWorld - edges[0] - edges[1] - edges[2]); + results.push_back(centerWorld + edges[0] - edges[1] - edges[2]); + results.push_back(centerWorld - edges[0] + edges[1] - edges[2]); + results.push_back(centerWorld + edges[0] + edges[1] - edges[2]); // this +,+,- + results.push_back(centerWorld - edges[0] - edges[1] + edges[2]); + results.push_back(centerWorld + edges[0] - edges[1] + edges[2]); //this +,-,+ + results.push_back(centerWorld - edges[0] + edges[1] + edges[2]); //this -,+,+ + results.push_back(centerWorld + edges[0] + edges[1] + edges[2]);//this +,+,+ + return results; + } + + // Get Rigid Box Size + inline XMVECTOR getBoxSize(const XMMATRIX& obj2World) + { + XMVECTOR size = XMVectorZero(); + XMVECTOR edges[3]; + for (size_t i = 0; i < 3; ++i){ + edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), 0.5f, i), obj2World); + XMVECTOR length = XMVector3Length(edges[i]); + + size = XMVectorSetByIndex(size, 2.0f*XMVectorGetByIndex(length, 0), i); + } + return size; + } + + // Get important Edges + inline std::vector getImportantEdges(const XMMATRIX& obj2World) + { + XMVECTOR xaxis = XMVectorSet(1, 0, 0, 1); + XMVECTOR yaxis = XMVectorSet(0, 1, 0, 1); + XMVECTOR zaxis = XMVectorSet(0, 0, 1, 1); + XMVECTOR edge1 = XMVector3TransformNormal(xaxis, obj2World); + XMVECTOR edge2 = XMVector3TransformNormal(yaxis, obj2World); + XMVECTOR edge3 = XMVector3TransformNormal(zaxis, obj2World); + std::vector results; + results.push_back(edge1); + results.push_back(edge2); + results.push_back(edge3); + return results; + } + + // Get the Normal to the faces + inline std::vector getAxisNormalToFaces(const XMMATRIX& obj2World) + { + std::vector edges; + XMVECTOR xaxis = XMVectorSet(1, 0, 0, 1); + XMVECTOR yaxis = XMVectorSet(0, 1, 0, 1); + XMVECTOR zaxis = XMVectorSet(0, 0, 1, 1); + XMVECTOR edge1 = XMVector3Normalize(XMVector3TransformNormal(xaxis, obj2World)); + XMVECTOR edge2 = XMVector3Normalize(XMVector3TransformNormal(yaxis, obj2World)); + XMVECTOR edge3 = XMVector3Normalize(XMVector3TransformNormal(zaxis, obj2World)); + std::vector results; + edges.push_back(edge1); + edges.push_back(edge2); + edges.push_back(edge3); + return edges; + } + + + // Get the pair of edges + inline std::vector getPairOfEdges(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B) + { + std::vector edges1 = getAxisNormalToFaces(obj2World_A); + std::vector edges2 = getAxisNormalToFaces(obj2World_B); + + std::vector results; + for (int i = 0; i < edges1.size(); i++) + { + for (int j = 0; j 0) + results.push_back(XMVector3Normalize(vector)); + } + } + return results; + } + + // project a shape on an axis + inline Projection project(const XMMATRIX& obj2World, XMVECTOR axis) + { + // Get corners + std::vector cornersWorld = getCorners(obj2World); + float min = XMVectorGetX(XMVector3Dot(cornersWorld[0], axis)); + float max = min; + for (int i = 1; i < cornersWorld.size(); i++) + { + float p = XMVectorGetX(XMVector3Dot(cornersWorld[i], axis)); + if (p < min) { + min = p; + } + else if (p > max) { + max = p; + } + } + Projection projection; + projection.max = max; + projection.min = min; + return projection; + } + + inline bool overlap(Projection p1, Projection p2) + { + return !((p1.max > p2.max && p1.min > p2.max) || (p2.max > p1.max && p2.min > p1.max)); + } + + inline float getOverlap(Projection p1, Projection p2) + { + return XMMin(p1.max, p2.max) - XMMax(p1.min, p2.min); + } + + static inline XMVECTOR contactPoint( + const XMVECTOR &pOne, + const XMVECTOR &dOne, + float oneSize, + const XMVECTOR &pTwo, + const XMVECTOR &dTwo, + float twoSize, + + // If this is true, and the contact point is outside + // the edge (in the case of an edge-face contact) then + // we use one's midpoint, otherwise we use two's. + bool useOne) + { + XMVECTOR toSt, cOne, cTwo; + float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo; + float denom, mua, mub; + + smOne = XMVectorGetX(XMVector3LengthSq(dOne)); + smTwo = XMVectorGetX(XMVector3LengthSq(dTwo)); + dpOneTwo = XMVectorGetX(XMVector3Dot(dTwo, dOne)); + + toSt = pOne - pTwo; + dpStaOne = XMVectorGetX(XMVector3Dot(dOne, toSt)); + dpStaTwo = XMVectorGetX(XMVector3Dot(dTwo, toSt)); + + denom = smOne * smTwo - dpOneTwo * dpOneTwo; + + // Zero denominator indicates parrallel lines + if (abs(denom) < 0.0001f) { + return useOne ? pOne : pTwo; + } + + mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom; + mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom; + + // If either of the edges has the nearest point out + // of bounds, then the edges aren't crossed, we have + // an edge-face contact. Our point is on the edge, which + // we know from the useOne parameter. + if (mua > oneSize || + mua < -oneSize || + mub > twoSize || + mub < -twoSize) + { + return useOne ? pOne : pTwo; + } + else + { + cOne = pOne + dOne * mua; + cTwo = pTwo + dTwo * mub; + + return cOne * 0.5 + cTwo * 0.5; + } + } + + inline XMVECTOR handleVertexToface(const XMMATRIX& obj2World, const XMVECTOR& toCenter) + { + std::vector corners = getCorners(obj2World); + float min = 1000; + XMVECTOR vertex; + for (int i = 0; i < corners.size(); i++) + { + float value = XMVectorGetX(XMVector3Dot(corners[i], toCenter)); + if (value < min) + { + vertex = corners[i]; + min = value; + } + } + + return vertex; + } + + + inline CollisionInfo checkCollisionSATHelper(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B, XMVECTOR size_A, XMVECTOR size_B) + { + CollisionInfo info; + info.isValid = false; + XMVECTOR collisionPoint = XMVectorZero(); + float smallOverlap = 10000.0f; + XMVECTOR axis; + int index; + int fromWhere = -1; + bool bestSingleAxis = false; + XMVECTOR toCenter = getVectorConnnectingCenters(obj2World_A, obj2World_B); + std::vector axes1 = getAxisNormalToFaces(obj2World_A); + std::vector axes2 = getAxisNormalToFaces(obj2World_B); + std::vector axes3 = getPairOfEdges(obj2World_A, obj2World_B); + // loop over the axes1 + for (int i = 0; i < axes1.size(); i++) { + // project both shapes onto the axis + Projection p1 = project(obj2World_A, axes1[i]); + Projection p2 = project(obj2World_B, axes1[i]); + // do the projections overlap? + if (!overlap(p1, p2)) { + // then we can guarantee that the shapes do not overlap + return info; + } + else{ + // get the overlap + float o = getOverlap(p1, p2); + // check for minimum + if (o < smallOverlap) { + // then set this one as the smallest + smallOverlap = o; + axis = axes1[i]; + index = i; + fromWhere = 0; + } + } + } + // loop over the axes2 + for (int i = 0; i < axes2.size(); i++) { + // project both shapes onto the axis + Projection p1 = project(obj2World_A, axes2[i]); + Projection p2 = project(obj2World_B, axes2[i]); + // do the projections overlap? + if (!overlap(p1, p2)) { + // then we can guarantee that the shapes do not overlap + return info; + } + else{ + // get the overlap + float o = getOverlap(p1, p2); + // check for minimum + if (o < smallOverlap) { + // then set this one as the smallest + smallOverlap = o; + axis = axes2[i]; + index = i; + fromWhere = 1; + bestSingleAxis = true; + } + } + } + int whichEdges = 0; + // loop over the axes3 + for (int i = 0; i < axes3.size(); i++) { + // project both shapes onto the axis + Projection p1 = project(obj2World_A, axes3[i]); + Projection p2 = project(obj2World_B, axes3[i]); + // do the projections overlap? + if (!overlap(p1, p2)) { + // then we can guarantee that the shapes do not overlap + return info; + } + else{ + // get the overlap + float o = getOverlap(p1, p2); + // check for minimum + if (o < smallOverlap) { + // then set this one as the smallest + smallOverlap = o; + axis = axes3[i]; + index = i; + whichEdges = i; + fromWhere = 2; + } + } + } + // if we get here then we know that every axis had overlap on it + // so we can guarantee an intersection + XMVECTOR normal; + switch (fromWhere){ + case 0:{ + normal = axis; + if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0) + { + normal = normal * -1.0f; + } + collisionPoint = handleVertexToface(obj2World_B, toCenter); + }break; + case 1:{ + normal = axis; + if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0) + { + normal = normal * -1.0f; + } + collisionPoint = handleVertexToface(obj2World_A, toCenter*-1); + }break; + case 2:{ + XMVECTOR axis = XMVector3Normalize(XMVector3Cross(axes1[whichEdges / 3], axes2[whichEdges % 3])); + normal = axis; + if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0) + { + normal = normal * -1.0f; + } + XMVECTOR ptOnOneEdge = XMVectorSet(0.5, 0.5, 0.5, 1); + XMVECTOR ptOnTwoEdge = XMVectorSet(0.5, 0.5, 0.5, 1); + + for (int i = 0; i < 3; i++) + { + if (i == whichEdges / 3) ptOnOneEdge = XMVectorSetByIndex(ptOnOneEdge, 0, i); + else if (XMVectorGetX(XMVector3Dot(axes1[i], normal)) < 0) ptOnOneEdge = XMVectorSetByIndex(ptOnOneEdge, -XMVectorGetByIndex(ptOnOneEdge, i), i); + + if (i == whichEdges % 3) ptOnTwoEdge = XMVectorSetByIndex(ptOnTwoEdge, 0, i); + else if (XMVectorGetX(XMVector3Dot(axes2[i], normal)) > 0) ptOnTwoEdge = XMVectorSetByIndex(ptOnTwoEdge, -XMVectorGetByIndex(ptOnTwoEdge, i), i); + } + ptOnOneEdge = XMVector3Transform(ptOnOneEdge, obj2World_A); + ptOnTwoEdge = XMVector3Transform(ptOnTwoEdge, obj2World_B); + collisionPoint = contactPoint(ptOnOneEdge, + axes1[whichEdges / 3], + (float)XMVectorGetByIndex(size_A, (whichEdges / 3)), + ptOnTwoEdge, + axes2[whichEdges % 3], + XMVectorGetByIndex(size_B, (whichEdges % 3)), + bestSingleAxis); + }break; + } + + + info.isValid = true; + info.collisionPointWorld = collisionPoint; + info.depth = smallOverlap; + info.normalWorld = normal*-1; + return info; + } +} + +/* params: +obj2World_A, the transfer matrix from object space of A to the world space +obj2World_B, the transfer matrix from object space of B to the world space +*/ +inline CollisionInfo checkCollisionSAT(GamePhysics::Mat4& obj2World_A, GamePhysics::Mat4& obj2World_B) { + using namespace collisionTools; + XMMATRIX MatA = obj2World_A.toDirectXMatrix(), MatB = obj2World_B.toDirectXMatrix(); + XMVECTOR calSizeA = getBoxSize(MatA); + XMVECTOR calSizeB = getBoxSize(MatB); + + return checkCollisionSATHelper(MatA, MatB, calSizeA, calSizeB); +} + +// example of using the checkCollisionSAT function +inline void testCheckCollision(int caseid){ + + if (caseid == 1){// simple examples, suppose that boxes A and B are cubes and have no rotation + GamePhysics::Mat4 AM; AM.initTranslation(1.0, 1.0, 1.0);// box A at (1.0,1.0,1.0) + GamePhysics::Mat4 BM; BM.initTranslation(2.0, 2.0, 2.0); //box B at (2.0,2.0,2.0) + + // check for collision + CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here + if (!simpletest.isValid) + std::printf("No Collision\n"); + else { + std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z); + std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z); + } + // case 1 result: + // collision detected at normal: -1.000000, -0.000000, -0.000000 + // collision point : 1.500000, 1.500000, 1.500000 + // Box A should be pushed to the left + } + else if (caseid == 2){// case 2, collide at a corner of Box B: + GamePhysics::Mat4 AM, BM; + AM.initTranslation(0.2f, 5.0f, 1.0f); // box A moves(0.2f, 5.0f, 1.0f) from origin + BM.initRotationZ(45); // box B rotates 45 degree around axis z + // box A size(9,2,3), box B size(5.656854f, 5.656854f, 2.0f) + GamePhysics::Mat4 SizeMat; + SizeMat.initScaling(9.0f, 2.0f, 3.0f); + AM = SizeMat * AM; + SizeMat.initScaling(5.656854f, 5.656854f, 2.0f); + BM = SizeMat * BM; + // check for collision + CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here + + if (!simpletest.isValid) + std::printf("No Collision\n"); + else { + std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z); + std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z); + } + // case 2 result: + // collision detected at normal : 0.000000, 1.000000, 0.000000 + // collision point : 0.000000, 4.000000, 1.000000 + } + else if (caseid == 3){// case 3, collide at a corner of Box A: + // box A first rotates 45 degree around axis z + // box A moves(-2.0f, 0.0f, 1.0f) from origin,(-2.0f,0.0f,1.0f) is the centre position of A in world space + // box A size(2.829f, 2.829f, 2.0f) + GamePhysics::Mat4 AM_rot; AM_rot.initRotationZ(45); + GamePhysics::Mat4 AM_tra; AM_tra.initTranslation(-2.0f, 0.0f, 1.0f); + GamePhysics::Mat4 AM_sca; AM_sca.initScaling(2.829f, 2.829f, 2.0f); + // get the object 2 world matrix of A + GamePhysics::Mat4 AM = AM_sca * AM_rot * AM_tra; // pay attention to the order! + // order, since we are working with the DirectX, we use left-handed matrixes! + + // box B first rotates 90 degree around axis z + // box B then moves (1.0f,0.5f,0.0f) from origin, (1.0f,0.5f,0.0f) is also the centre position of B in world space + // box B size(9.0f, 2.0f, 4.0f) + GamePhysics::Mat4 BM_rot; BM_rot.initRotationZ(90); + GamePhysics::Mat4 BM_tra; BM_tra.initTranslation(1.0f, 0.5f, 0.0f); + GamePhysics::Mat4 BM_sca; BM_sca.initScaling(9.0f, 2.0f, 4.0f); + GamePhysics::Mat4 BM = BM_sca * BM_rot * BM_tra; // pay attention to the order! + + // check for collision + CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here + + if (!simpletest.isValid) + std::printf("No Collision\n"); + else { + std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z); + std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z); + } + // case 3 result: + // collision detected at normal: -1.000000, 0.000000, -0.000000 + // collision point : 0.000405, 0.000000, 0.000000 + } +} diff --git a/Simulations/util/matrixbase.h b/Simulations/util/matrixbase.h index 2806dae..43bb9d7 100644 --- a/Simulations/util/matrixbase.h +++ b/Simulations/util/matrixbase.h @@ -646,8 +646,8 @@ matrix4x4::initRotationX(Scalar rot) this->initId(); value[1][1] = (Scalar) cos(drot); - value[2][1] = (Scalar) sin(drot); - value[1][2] = (Scalar)(-sin(drot)); + value[1][2] = (Scalar) sin(drot); + value[2][1] = (Scalar)(-sin(drot)); value[2][2] = (Scalar) cos(drot); } template @@ -659,8 +659,8 @@ matrix4x4::initRotationY(Scalar rot) this->initId(); value[0][0] = (Scalar) cos(drot); - value[2][0] = (Scalar)(-sin(drot)); - value[0][2] = (Scalar) sin(drot); + value[0][2] = (Scalar)(-sin(drot)); + value[2][0] = (Scalar) sin(drot); value[2][2] = (Scalar) cos(drot); } template @@ -672,8 +672,8 @@ matrix4x4::initRotationZ(Scalar rot) this->initId(); value[0][0] = (Scalar) cos(drot); - value[1][0] = (Scalar) sin(drot); - value[0][1] = (Scalar)(-sin(drot)); + value[0][1] = (Scalar) sin(drot); + value[1][0] = (Scalar)(-sin(drot)); value[1][1] = (Scalar) cos(drot); } template diff --git a/SimulatorTester/PublicRigidBodiesTests.cpp b/SimulatorTester/PublicRigidBodiesTests.cpp new file mode 100644 index 0000000..538a7c4 --- /dev/null +++ b/SimulatorTester/PublicRigidBodiesTests.cpp @@ -0,0 +1,105 @@ +#include "CppUnitTest.h" +#include "RigidBodySystemSimulator.h" + +using namespace Microsoft::VisualStudio::CppUnitTestFramework; + +namespace SimulatorTester +{ + TEST_CLASS(PublicRigidBodiesTests) + { + public: + void setupBaseTest(RigidBodySystemSimulator * rbss) { + rbss->m_iTestCase = TESTCASEUSEDTORUNTEST; + rbss->addRigidBody(Vec3(-0.1f, -0.2f, 0.1f), Vec3(0.4f, 0.2f, 0.2f), 100.0f); + + rbss->addRigidBody(Vec3(0.0f, 0.2f, 0.0f), Vec3(0.4f, 0.2f, 0.2f), 100.0); + rbss->setOrientationOf(1, Quat(Vec3(0.0f, 0.0f, 1.0f), (float)(M_PI)*0.25f)); + rbss->setVelocityOf(1,Vec3(0.0f, -0.1f, 0.05f)); + } + + TEST_METHOD(TestRigidBodiesInitialization) + { + RigidBodySystemSimulator * rbss = new RigidBodySystemSimulator(); + setupBaseTest(rbss); + Assert::AreEqual(2,(int)rbss->getNumberOfRigidBodies(),0.0001f,L"Number of Rigid bodies is not right",LINE_INFO()); + Vec3 pos = rbss->getPositionOfRigidBody(0); + Assert::AreEqual(-0.1f,(float)pos.x,0.0001f,L"X coordinate of body 0 is not right",LINE_INFO()); + Assert::AreEqual(-0.2f,(float)pos.y,0.0001f,L"Y coordinate of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.1f,(float)pos.z,0.0001f,L"Z coordinate of body 0 is not right",LINE_INFO()); + Vec3 vel = rbss->getLinearVelocityOfRigidBody(0); + Assert::AreEqual(0.0f,(float)vel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)vel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)vel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO()); + Vec3 angvel = rbss->getAngularVelocityOfRigidBody(0); + Assert::AreEqual(0.0f,(float)angvel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)angvel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)angvel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO()); + pos = rbss->getPositionOfRigidBody(1); + Assert::AreEqual(0.0f,(float)pos.x,0.0001f,L"X coordinate of body 1 is not right",LINE_INFO()); + Assert::AreEqual(0.2f,(float)pos.y,0.0001f,L"Y coordinate of body 1 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)pos.z,0.0001f,L"Z coordinate of body 1 is not right",LINE_INFO()); + vel = rbss->getLinearVelocityOfRigidBody(1); + Assert::AreEqual(0.0f,(float)vel.x,0.0001f,L"X componnent of body 1 is not right",LINE_INFO()); + Assert::AreEqual(-0.1f,(float)vel.y,0.0001f,L"Y componnent of body 1 is not right",LINE_INFO()); + Assert::AreEqual(0.05f,(float)vel.z,0.0001f,L"Z componnent of body 1 is not right",LINE_INFO()); + angvel = rbss->getAngularVelocityOfRigidBody(1); + Assert::AreEqual(0.0f,(float)angvel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)angvel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)angvel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO()); + delete rbss; + } + + TEST_METHOD(TestRigidBodiesAfterForceApplication) + { + RigidBodySystemSimulator * rbss = new RigidBodySystemSimulator(); + setupBaseTest(rbss); + rbss->applyForceOnBody(0,Vec3(0.0,0.0f,0.0),Vec3(0,0,200)); + for(int i =0; i < 4;i++) + rbss->simulateTimestep(0.1); + Vec3 pos = rbss->getPositionOfRigidBody(0); + Assert::AreEqual(-0.1f,(float)pos.x,0.0001f,L"X coordinate of body 0 is not right",LINE_INFO()); + Assert::AreEqual(-0.2f,(float)pos.y,0.0001f,L"Y coordinate of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.16f,(float)pos.z,0.0001f,L"Z coordinate of body 0 is not right",LINE_INFO()); + Vec3 vel = rbss->getLinearVelocityOfRigidBody(0); + Assert::AreEqual(0.0f,(float)vel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.0f,(float)vel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(0.2f,(float)vel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO()); + Vec3 angvel = rbss->getAngularVelocityOfRigidBody(0); + Assert::AreEqual(5.9064f,(float)angvel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(-1.7891f,(float)angvel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO()); + Assert::AreEqual(-1.0204f,(float)angvel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO()); + delete rbss; + } + + TEST_METHOD(TestRigidBodiesOneStepGivenTableTest) + { + RigidBodySystemSimulator * rbss = new RigidBodySystemSimulator(); + + rbss->m_iTestCase = TESTCASEUSEDTORUNTEST; + rbss->addRigidBody(Vec3(0.0f, 0.0f, 0.0f), Vec3(1.0f, 0.6f, 0.5f), 2.0f); + rbss->setOrientationOf(0, Quat(Vec3(0.0f, 0.0f, 1.0f), (float)(M_PI)* 0.5f)); + rbss->applyForceOnBody(0, Vec3(0.3f, 0.5f, 0.25f), Vec3(1.0f, 1.0f, 0.0f)); + rbss->simulateTimestep(2.0); + + Vec3 pos = rbss->getPositionOfRigidBody(0); + Assert::AreEqual(0.0000f, (float)pos.x, 0.0001f, L"X coordinate of position of body 0 is not right", LINE_INFO()); + Assert::AreEqual(0.0000f, (float)pos.y, 0.0001f, L"Y coordinate of position of body 0 is not right", LINE_INFO()); + Assert::AreEqual(0.0000f, (float)pos.z, 0.0001f, L"Z coordinate of position of body 0 is not right", LINE_INFO()); + Vec3 vel = rbss->getLinearVelocityOfRigidBody(0); + Assert::AreEqual(1.0000f, (float)vel.x, 0.0001f, L"X componnent of velocity of body 0 is not right", LINE_INFO()); + Assert::AreEqual(1.0000f, (float)vel.y, 0.0001f, L"Y componnent of velocity of body 0 is not right", LINE_INFO()); + Assert::AreEqual(0.0000f, (float)vel.z, 0.0001f, L"Z componnent of velocity of body 0 is not right", LINE_INFO()); + Vec3 angvel = rbss->getAngularVelocityOfRigidBody(0); + Assert::AreEqual(-2.4000f, (float)angvel.x, 0.0001f, L"X componnent of angular velocity of body 0 is not right", LINE_INFO()); + Assert::AreEqual(4.9180f, (float)angvel.y, 0.0001f, L"Y componnent of angular velocity of body 0 is not right", LINE_INFO()); + Assert::AreEqual(-1.7647f, (float)angvel.z, 0.0001f, L"Z componnent of angular velocity of body 0 is not right", LINE_INFO()); + + Vec3 xa_world = Vec3(-0.3f, -0.5f, -0.25f) - pos; + Vec3 velocityA = vel + cross(angvel, xa_world); + Assert::AreEqual(-1.11186f, (float)velocityA.x, 0.0001f, L"X componnent of the velocity at the given point is not right", LINE_INFO()); + Assert::AreEqual(0.929412f, (float)velocityA.y, 0.0001f, L"Y componnent of the velocity at the given point is not right", LINE_INFO()); + Assert::AreEqual(2.67541f, (float)velocityA.z, 0.0001f, L"Z componnent of the velocity at the given point is not right", LINE_INFO()); + delete rbss; + } + }; +} \ No newline at end of file diff --git a/SimulatorTester/SimulatorTester_2013.vcxproj b/SimulatorTester/SimulatorTester_2013.vcxproj index fe72e3c..62ced9e 100644 --- a/SimulatorTester/SimulatorTester_2013.vcxproj +++ b/SimulatorTester/SimulatorTester_2013.vcxproj @@ -1,193 +1,194 @@ - - - - - Debug - Win32 - - - Debug - x64 - - - Release - Win32 - - - Release - x64 - - - - {13342092-1CC5-4994-A03C-3963197E8016} - Win32Proj - SimulationsTester - SimulationsTester - - - - DynamicLibrary - true - v120 - Unicode - false - - - DynamicLibrary - true - v120 - Unicode - false - - - DynamicLibrary - false - v120 - true - Unicode - false - - - DynamicLibrary - false - v120 - true - Unicode - false - - - - - - - - - - - - - - - - - - - true - $(IncludePath) - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - - - true - $(IncludePath) - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - - - true - - - true - - - - NotUsing - Level3 - Disabled - $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) - WIN32;NOMINMAX;_CONSOLE;_DEBUG;%(PreprocessorDefinitions) - true - - - Windows - true - $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) - D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) - - - - - NotUsing - Level3 - Disabled - $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) - WIN32;NOMINMAX;_CONSOLE;_DEBUG;%(PreprocessorDefinitions) - true - - - Windows - true - $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) - D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) - - - - - Level3 - NotUsing - MaxSpeed - true - true - $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;%(PreprocessorDefinitions) - true - - - Windows - true - true - true - $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) - D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) - - - - - Level3 - NotUsing - MaxSpeed - true - true - $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;%(PreprocessorDefinitions) - true - - - Windows - true - true - true - $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) - D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) - - - - - - - - {b99e1fa1-c30a-45f2-9d57-9e9c21b2df42} - - - {e0b52ae7-e160-4d32-bf3f-910b785e5a8e} - - - {7329b02d-c504-482a-a156-181d48ce493c} - - - {85344b7f-5aa0-4e12-a065-d1333d11f6ca} - - - {61b333c2-c4f7-4cc1-a9bf-83f6d95588eb} - - - {df460eab-570d-4b50-9089-2e2fc801bf38} - - - {3cabed2c-12f1-4408-aaae-e2185a426f35} - - - - - + + + + + Debug + Win32 + + + Debug + x64 + + + Release + Win32 + + + Release + x64 + + + + {13342092-1CC5-4994-A03C-3963197E8016} + Win32Proj + SimulationsTester + SimulationsTester + + + + DynamicLibrary + true + v120 + Unicode + false + + + DynamicLibrary + true + v120 + Unicode + false + + + DynamicLibrary + false + v120 + true + Unicode + false + + + DynamicLibrary + false + v120 + true + Unicode + false + + + + + + + + + + + + + + + + + + + true + $(IncludePath) + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + + + true + $(IncludePath) + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + + + true + + + true + + + + NotUsing + Level3 + Disabled + $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) + WIN32;NOMINMAX;_CONSOLE;_DEBUG;%(PreprocessorDefinitions) + true + + + Windows + true + $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) + D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + NotUsing + Level3 + Disabled + $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) + WIN32;NOMINMAX;_CONSOLE;_DEBUG;%(PreprocessorDefinitions) + true + + + Windows + true + $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) + D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + NotUsing + MaxSpeed + true + true + $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;%(PreprocessorDefinitions) + true + + + Windows + true + true + true + $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) + D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + NotUsing + MaxSpeed + true + true + $(VCInstallDir)UnitTest\include;$(SolutionDir)Simulations;$(SolutionDir)DirectXTK/Inc;$(SolutionDir)DXUT11/Core;$(SolutionDir)DXUT11/Optional;$(SolutionDir)Effects11/inc;$(SolutionDir)AntTweakBar/include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;%(PreprocessorDefinitions) + true + + + Windows + true + true + true + $(VCInstallDir)UnitTest\lib;%(AdditionalLibraryDirectories) + D3DCompiler.lib;Mf.lib;mfuuid.lib;Mfplat.lib;Mfreadwrite.lib;comctl32.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + + + + {b99e1fa1-c30a-45f2-9d57-9e9c21b2df42} + + + {e0b52ae7-e160-4d32-bf3f-910b785e5a8e} + + + {7329b02d-c504-482a-a156-181d48ce493c} + + + {85344b7f-5aa0-4e12-a065-d1333d11f6ca} + + + {61b333c2-c4f7-4cc1-a9bf-83f6d95588eb} + + + {df460eab-570d-4b50-9089-2e2fc801bf38} + + + {3cabed2c-12f1-4408-aaae-e2185a426f35} + + + + + \ No newline at end of file diff --git a/SimulatorTester/SimulatorTester_2015.vcxproj b/SimulatorTester/SimulatorTester_2015.vcxproj index 8952a67..6725ea4 100644 --- a/SimulatorTester/SimulatorTester_2015.vcxproj +++ b/SimulatorTester/SimulatorTester_2015.vcxproj @@ -163,6 +163,7 @@ + diff --git a/SimulatorTester/SimulatorTester_2017.vcxproj b/SimulatorTester/SimulatorTester_2017.vcxproj index bd974ce..a9bef0c 100644 --- a/SimulatorTester/SimulatorTester_2017.vcxproj +++ b/SimulatorTester/SimulatorTester_2017.vcxproj @@ -164,6 +164,7 @@ +