virtual void Render(dFloat timeStep, DemoEntityManager* const scene) const
{
DemoEntity::Render(timeStep, scene);
ShowMousePicking (m_contact0, m_contact1, dVector (1, 0, 0, 0), dVector (0, 1, 0, 0));
ShowMousePicking (m_contact1, m_contact0, dVector (1, 0, 0, 0), dVector (0, 1, 0, 0));
}
void DemoCameraListener::RenderPickedTarget () const
{
if (m_targetPicked) {
dMatrix matrix;
NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);
dVector p0 (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
dVector p1 (p0 + matrix.RotateVector (m_pickedBodyLocalAtachmentNormal.Scale (0.5f)));
ShowMousePicking (p0, p1);
}
}
static int CalculateContacts (const NewtonBody* const otherBody, void* const userData)
{
ShowCollisionCollide* const me = (ShowCollisionCollide*)userData;
if (me->m_body != otherBody) {
const int cMaxContacts = 15;
dFloat contacts[cMaxContacts][3];
dFloat normals[cMaxContacts][3];
dFloat penetrations[cMaxContacts];
dLong attributeA[cMaxContacts];
dLong attributeB[cMaxContacts];
NewtonWorld* const world = NewtonBodyGetWorld(otherBody);
//NewtonBodyGetMatrix(me->m_body, &matrixA[0][0]);
//NewtonBodyGetMatrix(otherBody, &matrixB[0][0]);
DemoEntity* const entityA = (DemoEntity*)NewtonBodyGetUserData(me->m_body);
DemoEntity* const entityB = (DemoEntity*)NewtonBodyGetUserData(otherBody);
const dMatrix& matrixA = entityA->GetRenderMatrix ();
const dMatrix& matrixB = entityB->GetRenderMatrix ();
NewtonCollision* const collisionA = NewtonBodyGetCollision(me->m_body);
NewtonCollision* const collisionB = NewtonBodyGetCollision(otherBody);
int count = NewtonCollisionCollide (world, cMaxContacts, collisionA, &matrixA[0][0], collisionB, &matrixB[0][0],
&contacts[0][0], &normals[0][0], penetrations, attributeA, attributeB, 0);
dVector originColor (1.0f, 0.0f, 0.0f, 0.0f);
dVector lineColor (0.0f, 0.0f, 1.0f, 0.0f);
for (int i = 0; i < count; i ++) {
dVector n (normals[i][0], normals[i][1], normals[i][2], 0.0f);
dVector p0 (contacts[i][0], contacts[i][1], contacts[i][2], 0.0f);
dVector p1 (p0 + n.Scale (0.5f));
p0 = matrixA.UntransformVector(p0);
p1 = matrixA.UntransformVector(p1);
ShowMousePicking (p0, p1, originColor, lineColor);
}
}
return 1;
}
bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
witdh;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
// save point local to th body matrix
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// save the transform call back
chainForceCallback = NewtonBodyGetForceAndTorqueCallback (pickedBody);
dAssert (chainForceCallback != PhysicsApplyPickForce);
// set a new call back
NewtonBodySetForceAndTorqueCallback (pickedBody, PhysicsApplyPickForce);
}
}
if (mousePickMode) {
// init pick mode
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p2 (matrix.TransformVector (attachmentPoint));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
pickedForce = p - p2;
dFloat mag2 = pickedForce % pickedForce;
if (mag2 > dFloat (20 * 20)) {
pickedForce = pickedForce.Scale (20.0f / dSqrt (pickedForce % pickedForce));
}
// rotate normal to global space
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
// rayWorldOrigin = p2;
// show the pick points
//ShowMousePicking (p, p2, witdh);
ShowMousePicking (p, p2);
//ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length));
}
} else {
mousePickMode = false;
if (pickedBody) {
//dAssert (chainForceCallback != NewtonBodyGetForceAndTorqueCallback (pickedBody));
dAssert (chainForceCallback != PhysicsApplyPickForce);
NewtonBodySetForceAndTorqueCallback (pickedBody, chainForceCallback);
pickedBody = NULL;
chainForceCallback = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
bool MousePick (NewtonWorld* nWorld, const dMOUSE_POINT& mouse1, dInt32 mouseLeftKey1, dFloat witdh, dFloat length)
{
static int mouseLeftKey0;
static dMOUSE_POINT mouse0;
static bool mousePickMode = false;
dMatrix matrix;
static NewtonUserJoint* bodyPickController;
if (mouseLeftKey1) {
if (!mouseLeftKey0) {
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
pickedBody = NULL;
pickedParam = 1.1f;
isPickedBodyDynamics = false;
NewtonWorldRayCast(nWorld, &p0[0], &p1[0], RayCastFilter, NULL, RayCastPrefilter);
if (pickedBody) {
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
mousePickMode = true;
//NewtonBodySetFreezeState (pickedBody, 0);
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
dVector p (p0 + (p1 - p0).Scale (pickedParam));
attachmentPoint = matrix.UntransformVector (p);
// convert normal to local space
rayLocalNormal = matrix.UnrotateVector(rayLocalNormal);
// Create PickBody Joint
NewtonBodyGetMassMatrix (pickedBody, &mass, &Ixx, &Iyy, &Izz);
if (mass) {
// bodyPickController = new CustomPickBody (pickedBody, p);
// bodyPickController->SetMaxLinearFriction (MAX_PICK_ACCEL);
// bodyPickController->SetMaxAngularFriction (MAX_PICK_ACCEL * 5.0f);
bodyPickController = CreateCustomKinematicController (pickedBody, &p[0]);
CustomKinematicControllerSetMaxLinearFriction (bodyPickController, MAX_PICK_ACCEL);
CustomKinematicControllerSetMaxAngularFriction (bodyPickController, MAX_PICK_ACCEL * 5.0f);
}
}
}
if (mousePickMode) {
// init pick mode
dVector p0 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 0.0f, 0.0f)));
dVector p1 (ScreenToWorld(dVector (dFloat (mouse1.x), dFloat (mouse1.y), 1.0f, 0.0f)));
dVector p (p0 + (p1 - p0).Scale (pickedParam));
if (bodyPickController) {
//bodyPickController->SetTargetPosit (p);
CustomKinematicControllerSetTargetPosit (bodyPickController, &p[0]);
}
// rotate normal to global space
dMatrix matrix;
NewtonBodyGetMatrix(pickedBody, &matrix[0][0]);
rayWorldNormal = matrix.RotateVector(rayLocalNormal);
dVector p2 (matrix.TransformVector (attachmentPoint));
ShowMousePicking (p, p2, witdh);
ShowMousePicking (p2, p2 + rayWorldNormal.Scale (length), witdh);
}
} else {
mousePickMode = false;
if (pickedBody) {
if (bodyPickController) {
//delete bodyPickController;
CustomDestroyJoint (bodyPickController);
}
bodyPickController = NULL;
}
}
mouse0 = mouse1;
mouseLeftKey0 = mouseLeftKey1;
bool retState;
retState = isPickedBodyDynamics;
return retState;
}
