public unsafe static void GetHitNormalInWorld(this VehicleRaycasterResult obj, out OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
*(BulletSharp.Math.Vector3 *)valuePtr = obj.HitNormalInWorld;
}
}
public unsafe static void SetHitPointInWorld(this VehicleRaycasterResult obj, ref OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
obj.HitPointInWorld = *(BulletSharp.Math.Vector3 *)valuePtr;
}
}
public unsafe static object CastRay(this IVehicleRaycaster obj, ref OpenTK.Vector3 from, ref OpenTK.Vector3 to, VehicleRaycasterResult result)
{
fixed (OpenTK.Vector3* fromPtr = &from)
{
fixed (OpenTK.Vector3* toPtr = &to)
{
return obj.CastRay(ref *(BulletSharp.Math.Vector3*)fromPtr, ref *(BulletSharp.Math.Vector3*)toPtr, result);
}
}
}
public object CastRay(ref Vector3 from, ref Vector3 to, VehicleRaycasterResult result)
{
// RayResultCallback& resultCallback;
using (var rayCallback = new ClosestRayResultCallback(ref from, ref to))
{
_dynamicsWorld.RayTestRef(ref from, ref to, rayCallback);
if (rayCallback.HasHit)
{
var body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null && body.HasContactResponse)
{
result.HitPointInWorld = rayCallback.HitPointWorld;
Vector3 hitNormalInWorld = rayCallback.HitNormalWorld;
hitNormalInWorld.Normalize();
result.HitNormalInWorld = hitNormalInWorld;
result.DistFraction = rayCallback.ClosestHitFraction;
return(body);
}
}
}
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(source, 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;
}
rayResults.Dispose();
return depth;
}
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);
}
public Object CastRay(ref Vector3 from, ref Vector3 to, VehicleRaycasterResult result)
{
// RayResultCallback& resultCallback;
using (var rayCallback = new ClosestRayResultCallback(ref from, ref to))
{
m_dynamicsWorld.RayTestRef(ref from, ref to, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null && body.HasContactResponse)
{
result.HitPointInWorld = rayCallback.HitPointWorld;
Vector3 hitNormalInWorld = rayCallback.HitNormalWorld;
hitNormalInWorld.Normalize();
result.HitNormalInWorld = hitNormalInWorld;
result.DistFraction = rayCallback.ClosestHitFraction;
return body;
}
}
}
return null;
}
public unsafe static object CastRay(this IVehicleRaycaster obj, ref OpenTK.Vector3 from, ref OpenTK.Vector3 to, VehicleRaycasterResult result)
{
fixed(OpenTK.Vector3 *fromPtr = &from)
{
fixed(OpenTK.Vector3 *toPtr = &to)
{
return(obj.CastRay(ref *(BulletSharp.Math.Vector3 *)fromPtr, ref *(BulletSharp.Math.Vector3 *)toPtr, result));
}
}
}
public static void SetHitPointInWorld(this VehicleRaycasterResult obj, OpenTK.Vector3 value)
{
SetHitPointInWorld(obj, ref value);
}
public static OpenTK.Vector3 GetHitPointInWorld(this VehicleRaycasterResult obj)
{
OpenTK.Vector3 value;
GetHitPointInWorld(obj, out value);
return(value);
}
