void PhysicMap::rayCast(int srcID, const Triplet_f& start, const Triplet_f& end)
{
ShotInfo info(srcID, 0, start, end);
NewtonWorldRayCast(m_world, &start.x, &end.x, callbackFilter, &info, 00);
// TODO (#2#): line too long
CKernel::network()->sendShotFired(info.idShooter, info.idTarget, info.endPos);
}
void RayCastCompoundsAllSubShapes(const dVector& origin, const dVector& end)
{
m_param = 1.0f;
m_body = NULL;
NewtonWorld* const world = GetWorld();
NewtonWorldRayCast(world, &origin[0], &end[0], PickCompound, this, NULL, 0);
if (m_body) {
// we found a compound, find all sub shape on teh path of the ray
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector localP0(matrix.UntransformVector(origin));
dVector localP1(matrix.UntransformVector(end));
NewtonCollision* const compoundCollision = NewtonBodyGetCollision(m_body);
dAssert(NewtonCollisionGetType(compoundCollision) == SERIALIZE_ID_COMPOUND);
for (void* node = NewtonCompoundCollisionGetFirstNode(compoundCollision); node; node = NewtonCompoundCollisionGetNextNode(compoundCollision, node)) {
dVector normal;
dLong attribute;
NewtonCollision* const subShape = NewtonCompoundCollisionGetCollisionFromNode(compoundCollision, node);
dFloat xxx = NewtonCollisionRayCast(subShape, &localP0[0], &localP1[0], &normal[0], &attribute);
if (xxx < 1.0f) {
dTrace (("sub shape hit\n"))
}
}
}
}
NewtonBody* MousePickByForce (NewtonWorld* const nWorld, const dVector& origin, const dVector& end, dFloat& paramterOut, dVector& positionOut, dVector& normalOut)
{
dMousePickClass rayCast;
NewtonWorldRayCast(nWorld, &origin[0], &end[0], dMousePickClass::RayCastFilter, &rayCast, dMousePickClass::RayCastPrefilter, 0);
if (rayCast.m_body) {
positionOut = origin + (end - origin).Scale (rayCast.m_param);
normalOut = rayCast.m_normal;
paramterOut = rayCast.m_param;
}
return (NewtonBody*) rayCast.m_body;
}
dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist)
{
// shot a vertical ray from a high altitude and collect the intersection parameter.
dVector p0 (origin);
dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f));
dFloat parameter = 1.2f;
NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, ¶meter, RayPrefilter, 0);
if (parameter < 1.0f) {
p0 -= dVector (0.0f, dAbs (dist) * parameter, 0.0f, 0.0f);
}
return p0;
}
dFloat FindFloor (const NewtonWorld* world, dFloat x, dFloat z)
{
dFloat parameter;
// shot a vertical ray from a high altitude and collect the intersection parameter.
dVector p0 (x, 1000.0f, z);
dVector p1 (x, -1000.0f, z);
parameter = 1.2f;
NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, ¶meter, NULL);
//_ASSERTE (parameter < 1.0f);
// the intersection is the interpolated value
return 1000.0f - 2000.0f * parameter;
}
void Raycast::go(const OgreNewt::World* world, const Ogre::Vector3& startpt, const Ogre::Vector3& endpt )
{
if( world->getDebugger().isRaycastRecording() )
{
world->getDebugger().addRay(startpt, endpt);
}
m_treecollisioncallback_lastbody = NULL;
// perform the raycast!
NewtonWorldRayCast( world->getNewtonWorld(), (float*)&startpt, (float*)&endpt, OgreNewt::Raycast::newtonRaycastFilter, this, OgreNewt::Raycast::newtonRaycastPreFilter );
m_treecollisioncallback_lastbody = NULL;
}
dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist, dVector* const normal)
{
// shot a vertical ray from a high altitude and collect the intersection parameter.
dVector p0 (origin);
dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f));
RayCastPlacementData parameter;
NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, ¶meter, RayPrefilter, 0);
if (parameter.m_param < 1.0f) {
p0 -= dVector (0.0f, dAbs (dist) * parameter.m_param, 0.0f, 0.0f);
if (normal) {
*normal = parameter.m_normal;
}
}
return p0;
}
const ray::hitList &ray::operator()(const world &w,
const vec3 &p0, const vec3 &p1, ray::specificsMode sm)
{
_p0 = p0;
_p1 = p1;
_sm = sm;
_hitCache.clear();
NewtonWorldRayCast(w, &_p0.x, &_p1.x, &hitCB, this, &preHitCB);
specifics.clear();
// sort based on distance
std::sort(_hitCache.begin(), _hitCache.end(), hitInfoLess());
return _hitCache;
}
void CustomDGRayCastCar::CalculateTireCollision (Tire& tire, const dMatrix& suspensionMatrix, int threadIndex) const
{
int floorcontact = 0;
tire.m_HitBody = NULL;
tire.m_posit = tire.m_suspensionLenght;
if ( tire.m_tireUseConvexCastMode ) {
dFloat hitParam;
NewtonWorldConvexCastReturnInfo info;
// tire.m_rayDestination = tire.m_suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght ));
dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght)));
if ( NewtonWorldConvexCast ( m_world, &suspensionMatrix[0][0], &rayDestination[0], tire.m_shape, &hitParam, (void*)m_body0, ConvexCastPrefilter, &info, 1, threadIndex ) ){
tire.m_posit = hitParam * tire.m_suspensionLenght;
tire.m_contactPoint = info.m_point;
tire.m_contactNormal = info.m_normal;
tire.m_HitBody = (NewtonBody*)info.m_hitBody;
floorcontact = 1;
}
} else {
struct RayCastInfo
{
RayCastInfo (const NewtonBody* body)
{
m_param = 1.0f;
m_me = body;
m_hitBody = NULL;
m_contactID = 0;
m_normal = dVector (0.0f, 0.0f, 0.0f, 1.0f);
}
static dFloat RayCast (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
{
RayCastInfo& caster = *( (RayCastInfo*) userData );
// if this body is not the vehicle, see if a close hit
if ( body != caster.m_me ) {
if ( intersetParam < caster.m_param) {
// this is a close hit, record the information.
caster.m_param = intersetParam;
caster.m_hitBody = body;
caster.m_contactID = collisionID;
caster.m_normal = dVector (normal[0], normal[1], normal[2], 1.0f);
}
}
return intersetParam;
}
dFloat m_param;
dVector m_normal;
const NewtonBody* m_me;
const NewtonBody* m_hitBody;
int m_contactID;
};
RayCastInfo info (m_body0);
_ASSERTE (0);
// extend the ray by the radius of the tire
dFloat dist ( tire.m_suspensionLenght + tire.m_radius );
dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -dist )));
// cast a ray to the world ConvexCastPrefilter
NewtonWorldRayCast( m_world, &suspensionMatrix.m_posit[0], &rayDestination[0], RayCastInfo::RayCast, &info, &ConvexCastPrefilter );
// if the ray hit something, it means the tire has some traction
if ( info.m_hitBody ) {
dFloat intesectionDist;
tire.m_HitBody = (NewtonBody*)info.m_hitBody;
tire.m_contactPoint = suspensionMatrix.m_posit + (rayDestination - suspensionMatrix.m_posit ).Scale ( info.m_param );
tire.m_contactNormal = info.m_normal;
// TO DO: get the material properties for tire frictions on different roads
intesectionDist = ( dist * info.m_param - tire.m_radius );
if ( intesectionDist < 0.0f ) {
intesectionDist = 0.0f;
} else if ( intesectionDist > tire.m_suspensionLenght ) {
intesectionDist = tire.m_suspensionLenght;
}
tire.m_posit = intesectionDist;
}
}
}
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;
}
void dNewtonRayCast::CastRay (const dFloat* const p0, const dFloat* const p1, int threadIndex)
{
NewtonWorldRayCast (m_world->GetNewton(), p0, p1, RayFilterCallback, this, PrefilterCallback, threadIndex);
}
