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"); } }