/// <summary>
/// Finds the nearest RigidBody that doesn't belong to the robot.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="result"></param>
/// <returns></returns>
public object CastRay(ref Vector3 from, ref Vector3 to, VehicleRaycasterResult result)
{
AllHitsRayResultCallback rayCallback = new AllHitsRayResultCallback(from, to);
dynamicsWorld.RayTest(from, to, rayCallback);
int i = 0;
foreach (CollisionObject co in rayCallback.CollisionObjects)
{
BRigidBody brb = co.UserObject as BRigidBody;
if (brb != null)
{
if (!brb.gameObject.name.StartsWith("node_"))
{
result.HitPointInWorld = rayCallback.HitPointWorld[i];
result.HitNormalInWorld = rayCallback.HitNormalWorld[i];
result.HitNormalInWorld.Normalize();
result.DistFraction = rayCallback.HitFractions[i];
return(RigidBody.Upcast(co));
}
}
i++;
}
return(null);
}
private float RayCast(WheelInfo wheel)
{
UpdateWheelTransformsWS(wheel, false);
float depth = -1;
float raylen = wheel.SuspensionRestLength + wheel.WheelsRadius;
Vector3 rayvector = wheel.RaycastInfo.WheelDirectionWS * raylen;
Vector3 source = wheel.RaycastInfo.HardPointWS;
wheel.RaycastInfo.ContactPointWS = source + rayvector;
Vector3 target = wheel.RaycastInfo.ContactPointWS;
float param = 0;
VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
Debug.Assert(vehicleRaycaster != null);
object obj = vehicleRaycaster.CastRay(ref source, ref target, rayResults);
wheel.RaycastInfo.GroundObject = null;
if (obj != null)
{
param = rayResults.DistFraction;
depth = raylen * rayResults.DistFraction;
wheel.RaycastInfo.ContactNormalWS = rayResults.HitNormalInWorld;
wheel.RaycastInfo.IsInContact = true;
wheel.RaycastInfo.GroundObject = fixedBody;///@todo for driving on dynamic/movable objects!;
/////wheel.RaycastInfo.GroundObject = object;
float hitDistance = param * raylen;
wheel.RaycastInfo.SuspensionLength = hitDistance - wheel.WheelsRadius;
//clamp on max suspension travel
float minSuspensionLength = wheel.SuspensionRestLength - wheel.MaxSuspensionTravelCm * 0.01f;
float maxSuspensionLength = wheel.SuspensionRestLength + wheel.MaxSuspensionTravelCm * 0.01f;
if (wheel.RaycastInfo.SuspensionLength < minSuspensionLength)
{
wheel.RaycastInfo.SuspensionLength = minSuspensionLength;
}
if (wheel.RaycastInfo.SuspensionLength > maxSuspensionLength)
{
wheel.RaycastInfo.SuspensionLength = maxSuspensionLength;
}
wheel.RaycastInfo.ContactPointWS = rayResults.HitPointInWorld;
float denominator = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, wheel.RaycastInfo.WheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = wheel.RaycastInfo.ContactPointWS - RigidBody.CenterOfMassPosition;
chassis_velocity_at_contactPoint = RigidBody.GetVelocityInLocalPoint(relpos);
float projVel = Vector3.Dot(wheel.RaycastInfo.ContactNormalWS, chassis_velocity_at_contactPoint);
if (denominator >= -0.1f)
{
wheel.SuspensionRelativeVelocity = 0;
wheel.ClippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1.0f / denominator;
wheel.SuspensionRelativeVelocity = projVel * inv;
wheel.ClippedInvContactDotSuspension = inv;
}
}
else
{
//put wheel info as in rest position
wheel.RaycastInfo.SuspensionLength = wheel.SuspensionRestLength;
wheel.SuspensionRelativeVelocity = 0.0f;
wheel.RaycastInfo.ContactNormalWS = -wheel.RaycastInfo.WheelDirectionWS;
wheel.ClippedInvContactDotSuspension = 1.0f;
}
return(depth);
}
