void MatrixTest::testMatrixMatrixOperations ()
{
Matrix<2,3,int> lMatrix;
Matrix<3,2,int> rMatrix;
Matrix<2,2,int> result(0);
assignList(lMatrix) = 1, 2, 3,
4, 5, 6;
assignList(rMatrix) = 6, 5,
4, 3,
2, 1;
// Matrix matrix multiplication
multiply (lMatrix, rMatrix, result);
validateEquals (result(0,0), 20);
validateEquals (result(0,1), 14);
validateEquals (result(1,0), 56);
validateEquals (result(1,1), 41);
// Bitwise comparison
Matrix<2,3,int> matrixA(1);
Matrix<2,3,int> matrixB(2);
validate (matrixA == matrixA);
validate (! (matrixA == matrixB));
// Test equalsReturnIndex
Matrix<2,3,double> matrix1(1);
Matrix<2,3,double> matrix2(2);
int i=equalsReturnIndex(matrix1,matrix2);
validateEquals(i,0);
}
TEST(Matrix, MatrixMultiplication)
{
core::F32_t valuesA[4][4];
core::F32_t valuesB[4][4];
core::F32_t valueA = 0;
core::F32_t valueB = 15;
for(int i = 0; i < 4; i++)
{
for(int j = 0; j < 4; j++)
{
valuesA[i][j] = valueA;
valuesB[i][j] = valueB;
valueA++;
valueB--;
}
}
core::Matrix4x4 matrixA(valuesA);
core::Matrix4x4 matrixB(valuesB);
core::Matrix4x4 result;
core::Matrix4x4::multiply(matrixA, matrixB, result);
ASSERT_EQ(result.m[0][0], 34);
ASSERT_EQ(result.m[0][1], 28);
ASSERT_EQ(result.m[0][2], 22);
ASSERT_EQ(result.m[0][3], 16);
ASSERT_EQ(result.m[1][0], 178);
ASSERT_EQ(result.m[1][1], 156);
ASSERT_EQ(result.m[1][2], 134);
ASSERT_EQ(result.m[1][3], 112);
ASSERT_EQ(result.m[2][0], 322);
ASSERT_EQ(result.m[2][1], 284);
ASSERT_EQ(result.m[2][2], 246);
ASSERT_EQ(result.m[2][3], 208);
ASSERT_EQ(result.m[3][0], 466);
ASSERT_EQ(result.m[3][1], 412);
ASSERT_EQ(result.m[3][2], 358);
ASSERT_EQ(result.m[3][3], 304);
}
void PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrixA;
NewtonBodyGetMatrix(m_body, &matrixA[0][0]);
dFloat speed = m_step * timestep * 60.0f;
m_pith = dMod (m_pith + speed, 3.1416f * 2.0f);
m_yaw = dMod (m_yaw + speed, 3.1416f * 2.0f);
m_roll = dMod (m_roll + speed, 3.1416f * 2.0f);
dMatrix matrixB(dPitchMatrix(m_pith) * dYawMatrix(m_yaw) * dRollMatrix(m_roll));
matrixB.m_posit = matrixA.m_posit;
matrixB.m_posit.m_y = 5.0f;
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData (world);
m_castingVisualEntity->ResetMatrix(*scene, matrixB);
NewtonCollision* const collisionA = NewtonBodyGetCollision(m_body);
NewtonCollisionClosestPoint(world, collisionA, &matrixA[0][0], m_castingVisualEntity->m_castingShape, &matrixB[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_contact1[0], &m_castingVisualEntity->m_normal[0], 0);
}
void PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrixA;
NewtonBodyGetMatrix(m_body, &matrixA[0][0]);
dFloat speed = m_step * timestep * 60.0f;
m_pith = dMod (m_pith + speed, dPi * 2.0f);
m_yaw = dMod (m_yaw + speed, dPi * 2.0f);
m_roll = dMod (m_roll + speed, dPi * 2.0f);
dMatrix matrixB(dPitchMatrix(m_pith) * dYawMatrix(m_yaw) * dRollMatrix(m_roll));
matrixB.m_posit = matrixA.m_posit;
matrixB.m_posit.m_y = 5.0f;
//matrixB.m_posit.m_y = 1.5f;
NewtonWorld* const world = NewtonBodyGetWorld(m_body);
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData (world);
m_castingVisualEntity->ResetMatrix(*scene, matrixB);
NewtonCollision* const collisionA = NewtonBodyGetCollision(m_body);
int res = NewtonCollisionClosestPoint(world, collisionA, &matrixA[0][0], m_castingVisualEntity->m_castingShape, &matrixB[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_contact1[0], &m_castingVisualEntity->m_normal[0], 0);
//just test the center of collisionB against collisionA to see if the point is inside or not:
//int res = NewtonCollisionPointDistance(world, &matrixA.m_posit[0], collisionA, &matrixA[0][0], &m_castingVisualEntity->m_contact0[0], &m_castingVisualEntity->m_normal[0], 0);
if (res == 0) {
printf("Point Inside Body!\n");
//dTrace(("Point Inside Body!\n"));
}
else
{
//printf("Point point outside Body!\n");
}
}
