//////////////////////////////////////////////////////////////////////////
// Position the body precisely
//////////////////////////////////////////////////////////////////////////
void DropDownBox(NewtonBody *body)
{
float matrix[16];
NewtonBodyGetMatrix(body, matrix);
Matrix4 m = Matrix4(matrix);
Vector3 pos = m.GetPosition();
pos.y += 1.0f;
m.SetPosition(pos);
Vector3 p = pos;
p.y -= 20;
// cast collision shape within +20 -20 y range
NewtonWorld *world = NewtonBodyGetWorld(body);
NewtonCollision *collision = NewtonBodyGetCollision(body);
float param;
NewtonWorldConvexCastReturnInfo info[16];
m.FlattenToArray(matrix);
NewtonWorldConvexCast(world, matrix, &p[0], collision, ¶m, body, DropDownConvexCastCallback, info, 16, 0);
m = Matrix4(matrix);
pos = m.GetPosition();
m.SetPosition(pos + Vector3(0, (p.y - pos.y) * param, 0) );
m.FlattenToArray(matrix);
NewtonBodySetMatrix(body, matrix);
}
void CPhysics::TransformCallback(const NewtonBody* body, const float* matrix, int threadIndex)
{
CObject3D *object = (CObject3D*)NewtonBodyGetUserData(body);
float m[16];
NewtonBodyGetMatrix(body, m);
Matrix4 M = Matrix4(m);
Vector3 destination = M.GetPosition();
Vector3 velocity = (destination - object->GetWorldPosition()) * object->physicsDelay;
object->velocity = velocity;
Vector3 pos = object->position;
object->SetMatrix( Matrix4(m) );
object->SetPosition(pos);
// setting entire matrix each physics frame might cause desync with the renderer,
// that might run more often, resulting in drawing the object in the same location
// twice in a row
// Instead each physics frame we set the object's velocity so that it reaches
// the desginated position in 1/10th of a second
}