public virtual Object CastRay(ref Vector3 from,ref Vector3 to, ref VehicleRaycasterResult result)
{
// RayResultCallback& resultCallback;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref from,ref to);
m_dynamicsWorld.RayTest(ref from, ref to, rayCallback);
if (rayCallback.HasHit())
{
RigidBody body = RigidBody.Upcast(rayCallback.m_collisionObject);
if (body != null && body.HasContactResponse())
{
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
result.m_hitNormalInWorld.Normalize();
result.m_distFraction = rayCallback.m_closestHitFraction;
return body;
}
}
else
{
int ibreak = 0;
ClosestRayResultCallback rayCallback2 = new ClosestRayResultCallback(ref from, ref to);
m_dynamicsWorld.RayTest(ref from, ref to, rayCallback2);
}
rayCallback.Cleanup();
return null;
}
public float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.GetSuspensionRestLength() + wheel.m_wheelsRadius;
Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
Vector3 source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
Vector3 target = wheel.m_raycastInfo.m_contactPointWS;
float param = 0f;
VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
Debug.Assert(m_vehicleRaycaster != null);
Object object1 = m_vehicleRaycaster.CastRay(ref source, ref target, ref rayResults);
if (object1 != null && ((RigidBody)object1).m_debugBodyId != 1)
{
int ibreak = 0;
}
//{
// Vector3 from1 = new Vector3(0.7957098f, -9.13606f, 1.794605f);
// Vector3 to1 = new Vector3(0.886791f, -10.23207f, 1.815941f);
// VehicleRaycasterResult results1 = new VehicleRaycasterResult();
// Object o2 = m_vehicleRaycaster.castRay(ref from1, ref to1, ref results1);
// Vector3 from2 = new Vector3(0.7957281f, -9.136093f, 1.794625f);
// Vector3 to2 = new Vector3(0.8867911f, -10.23211f, 1.815956f);
// VehicleRaycasterResult results2 = new VehicleRaycasterResult();
// Object o3 = m_vehicleRaycaster.castRay(ref from2, ref to2, ref results2);
// if (Math.Abs(results1.m_distFraction - results2.m_distFraction) > 0.1f)
// {
// int ibreak = 0;
// }
//}
wheel.m_raycastInfo.m_groundObject = null;
if (object1 != null)
{
param = rayResults.m_distFraction;
depth = raylen * rayResults.m_distFraction;
wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
wheel.m_raycastInfo.m_isInContact = true;
wheel.m_raycastInfo.m_groundObject = s_fixedObject;///@todo for driving on dynamic/movable objects!;
//wheel.m_raycastInfo.m_groundObject = object;
float hitDistance = param * raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
//clamp on max suspension travel
float minSuspensionLength = wheel.GetSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * 0.01f;
float maxSuspensionLength = wheel.GetSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * 0.01f;
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
}
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
{
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
}
if (System.Math.Abs(wheel.m_raycastInfo.m_suspensionLength - wheel.m_raycastInfo.m_suspensionLengthBak) > 0.1f)
{
int ibreak = 0;
}
wheel.m_raycastInfo.m_suspensionLengthBak = wheel.m_raycastInfo.m_suspensionLength;
wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
float denominator = Vector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, wheel.m_raycastInfo.m_wheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - GetRigidBody().GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = GetRigidBody().GetVelocityInLocalPoint(ref relpos);
float projVel = Vector3.Dot(wheel.m_raycastInfo.m_contactNormalWS, chassis_velocity_at_contactPoint);
if (projVel > 1f)
{
int ibreak = 0;
}
if (denominator >= -0.1f)
{
wheel.m_suspensionRelativeVelocity = 0f;
wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1f / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
}
else
{
//put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.GetSuspensionRestLength();
wheel.m_suspensionRelativeVelocity = 0.0f;
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = 1.0f;
}
return depth;
}
public DataCopy(Vector3 from,Vector3 to,VehicleRaycasterResult result)
{
m_from = from;
m_to = to;
m_result = result;
}