VALUE MSNewton::Bodies::get_closest_points(VALUE self, VALUE v_body1, VALUE v_body2) {
const NewtonBody* body1 = Util::value_to_body(v_body1);
const NewtonBody* body2 = Util::value_to_body(v_body2);
Util::validate_two_bodies(body1, body2);
const NewtonWorld* world = NewtonBodyGetWorld(body1);
WorldData* world_data = (WorldData*)NewtonWorldGetUserData(world);
NewtonCollision* colA = NewtonBodyGetCollision(body1);
NewtonCollision* colB = NewtonBodyGetCollision(body2);
dMatrix matrixA;
dMatrix matrixB;
NewtonBodyGetMatrix(body1, &matrixA[0][0]);
NewtonBodyGetMatrix(body2, &matrixB[0][0]);
dVector pointA;
dVector pointB;
dVector normalAB;
if (NewtonCollisionClosestPoint(world, colA, &matrixA[0][0], colB, &matrixB[0][0], &pointA[0], &pointB[0], &normalAB[0], 0) == 0)
return Qnil;
return rb_ary_new3(2, Util::point_to_value(pointA, world_data->inverse_scale), Util::point_to_value(pointB, world_data->inverse_scale));
}
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");
}
}