kookroach/game-physics
1
0
Fork 0
Code Issues 8 Pull Requests Packages Projects 1 Releases Wiki Activity

for Ex2

Browse Source
This commit is contained in:
youxie
2018-11-06 07:36:13 +01:00
parent 43628b8f74
commit 25f977e43d
7 changed files with 829 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
Simulations/RigidBodySystemSimulator.h Normal file
View File

@@ -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

477
Simulations/collisionDetect.h Normal file
View File

@@ -0,0 +1,477 @@
// header file:
#include <DirectXMath.h>
#include <Vector>
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<XMVECTOR> discritizeObject(const XMMATRIX& obj2World)
{
const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World);
XMVECTOR edges[3];
std::vector<XMVECTOR> 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<XMVECTOR> 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<XMVECTOR> 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<XMVECTOR> 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<XMVECTOR> results;
results.push_back(edge1);
results.push_back(edge2);
results.push_back(edge3);
return results;
}
// Get the Normal to the faces
inline std::vector<XMVECTOR> getAxisNormalToFaces(const XMMATRIX& obj2World)
{
std::vector<XMVECTOR> 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<XMVECTOR> results;
edges.push_back(edge1);
edges.push_back(edge2);
edges.push_back(edge3);
return edges;
}
// Get the pair of edges
inline std::vector<XMVECTOR> getPairOfEdges(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B)
{
std::vector<XMVECTOR> edges1 = getAxisNormalToFaces(obj2World_A);
std::vector<XMVECTOR> edges2 = getAxisNormalToFaces(obj2World_B);
std::vector<XMVECTOR> results;
for (int i = 0; i < edges1.size(); i++)
{
for (int j = 0; j<edges2.size(); j++)
{
XMVECTOR vector = XMVector3Cross(edges1[i], edges2[j]);
if (XMVectorGetX(XMVector3Length(vector)) > 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<XMVECTOR> 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<XMVECTOR> 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<XMVECTOR> axes1 = getAxisNormalToFaces(obj2World_A);
std::vector<XMVECTOR> axes2 = getAxisNormalToFaces(obj2World_B);
std::vector<XMVECTOR> 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
}
}

12
Simulations/util/matrixbase.h
View File

@@ -646,8 +646,8 @@ matrix4x4<Scalar>::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<class Scalar>
@@ -659,8 +659,8 @@ matrix4x4<Scalar>::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<class Scalar>
@@ -672,8 +672,8 @@ matrix4x4<Scalar>::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<class Scalar>

105
SimulatorTester/PublicRigidBodiesTests.cpp Normal file
View File

@@ -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;
}
};
}

1
SimulatorTester/SimulatorTester_2013.vcxproj
View File

@@ -163,6 +163,7 @@
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="PublicMassSpringSystemTests.cpp" />
<ClCompile Include="PublicRigidBodiesTests.cpp" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\AntTweakBar\src\AntTweakBar_2013.vcxproj">

1
SimulatorTester/SimulatorTester_2015.vcxproj
View File

@@ -163,6 +163,7 @@
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="PublicMassSpringSystemTests.cpp" />
<ClCompile Include="PublicRigidBodiesTests.cpp" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\AntTweakBar\src\AntTweakBar_2015.vcxproj">

1
SimulatorTester/SimulatorTester_2017.vcxproj
View File

@@ -164,6 +164,7 @@
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="PublicMassSpringSystemTests.cpp" />
<ClCompile Include="PublicRigidBodiesTests.cpp" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\AntTweakBar\src\AntTweakBar_2017.vcxproj">