/// <summary>
/// Adds a wheel to the RaycastRobot.
/// </summary>
/// <param name="connectionPointCS"></param>
/// <param name="wheelDirectionCS0"></param>
/// <param name="wheelAxleCS"></param>
/// <param name="suspensionRestLength"></param>
/// <param name="wheelRadius"></param>
/// <param name="tuning"></param>
/// <param name="isFrontWheel"></param>
/// <returns></returns>
public RobotWheelInfo AddWheel(Vector3 connectionPointCS, Vector3 wheelDirectionCS0, Vector3 wheelAxleCS,
float suspensionRestLength, float wheelRadius, VehicleTuning tuning, bool isFrontWheel)
{
WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo
{
ChassisConnectionCS = connectionPointCS,
WheelDirectionCS = wheelDirectionCS0,
WheelAxleCS = wheelAxleCS,
SuspensionRestLength = suspensionRestLength,
WheelRadius = wheelRadius,
SuspensionStiffness = tuning.SuspensionStiffness,
WheelsDampingCompression = tuning.SuspensionCompression,
WheelsDampingRelaxation = tuning.SuspensionDamping,
FrictionSlip = tuning.FrictionSlip,
IsFrontWheel = isFrontWheel,
MaxSuspensionTravelCm = tuning.MaxSuspensionTravelCm,
MaxSuspensionForce = tuning.MaxSuspensionForce
};
Array.Resize(ref wheelInfo, wheelInfo.Length + 1);
RobotWheelInfo wheel = new RobotWheelInfo(ci);
wheelInfo[wheelInfo.Length - 1] = wheel;
UpdateWheelTransformsWS(wheel, false);
UpdateWheelTransform(NumWheels - 1, false);
return(wheel);
}
/// <summary>
/// Adds a wheel to the BRaycastVehicle from the given information.
/// </summary>
/// <param name="connectionPoint"></param>
/// <param name="axle"></param>
/// <param name="suspensionRestLength"></param>
/// <param name="radius"></param>
/// <returns></returns>
public int AddWheel(WheelType wheelType, BulletSharp.Math.Vector3 connectionPoint, BulletSharp.Math.Vector3 axle, float suspensionRestLength, float radius)
{
float slidingFriction = DefaultSlidingFriction;
switch (wheelType)
{
case WheelType.MECANUM:
slidingFriction = 0.1f;
axle = (Quaternion.AngleAxis((connectionPoint.X > 0 && connectionPoint.Z > 0) || (connectionPoint.X < 0 && connectionPoint.Z < 0) ? -45 : 45,
Vector3.up) * axle.ToUnity()).ToBullet();
break;
case WheelType.OMNI:
slidingFriction = 0.1f;
break;
}
RobotWheelInfo wheel = RaycastRobot.AddWheel(connectionPoint,
rootNode.WheelsNormal.ToBullet(), axle, suspensionRestLength,
radius, defaultVehicleTuning, false);
wheel.RollInfluence = RollInfluence;
wheel.SlidingFriction = slidingFriction;
return(RaycastRobot.NumWheels - 1);
}
/// <summary>
/// Updates the position of the wheel according to the BRaycastVehicle's position and speed.
/// </summary>
private void Update()
{
if (robot == null)
{
return;
}
RobotWheelInfo wheel = robot.RaycastRobot.GetWheelInfo(wheelIndex);
if (wheel.Brake == 0f)
{
wheel.Brake = (RollingFriction / radius) * robot.RaycastRobot.OverrideMass * MassTorqueScalar;
}
if (updatePosition)
{
transform.position = wheel.WorldTransform.Origin.ToUnity();
}
transform.localRotation *= Quaternion.AngleAxis(-wheel.Speed, axis);
}
/// <summary>
/// Updates the friction for the RaycastRobot.
/// </summary>
/// <param name="timeStep"></param>
public void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = NumWheels;
if (numWheel == 0)
{
return;
}
Array.Resize <Vector3>(ref forwardWS, numWheel);
Array.Resize <Vector3>(ref axle, numWheel);
Array.Resize <float>(ref forwardImpulse, numWheel);
Array.Resize <float>(ref sideImpulse, numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < NumWheels; i++)
{
RigidBody groundObject = wheelInfo[i].RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
sideImpulse[i] = 0;
forwardImpulse[i] = 0;
}
for (int i = 0; i < NumWheels; i++)
{
RobotWheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
if (groundObject != null)
{
Matrix wheelTrans = GetWheelTransformWS(i);
axle[i] = new Vector3(
wheelTrans[0, RightAxis],
wheelTrans[1, RightAxis],
wheelTrans[2, RightAxis]);
Vector3 surfNormalWS = wheel.RaycastInfo.ContactNormalWS;
float proj;
Vector3.Dot(ref axle[i], ref surfNormalWS, out proj);
axle[i] -= surfNormalWS * proj;
axle[i].Normalize();
Vector3.Cross(ref surfNormalWS, ref axle[i], out forwardWS[i]);
forwardWS[i].Normalize();
ResolveSingleBilateral(RootRigidBody, wheel.RaycastInfo.ContactPointWS,
groundObject, wheel.RaycastInfo.ContactPointWS,
0, axle[i], ref sideImpulse[i], timeStep);
sideImpulse[i] *= wheel.SlidingFriction;
}
else
{
if (wheel.Speed > 0)
{
wheel.Speed = Math.Max(wheel.Speed - wheel.FreeSpinDamping, 0f);
}
else if (wheel.Speed < 0)
{
wheel.Speed = Math.Min(wheel.Speed + wheel.FreeSpinDamping, 0f);
}
}
}
bool sliding = false;
for (int i = 0; i < NumWheels; i++)
{
RobotWheelInfo wheel = wheelInfo[i];
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
float rollingFriction = 0.0f;
if (groundObject != null)
{
Vector3 velocity = RigidBody.GetVelocityInLocalPoint(wheel.ChassisConnectionPointCS);
Vector3 localVelocity = Vector3.TransformNormal(velocity, Matrix.Invert(RigidBody.WorldTransform.Basis));
Vector3 forwardAxis = (UnityEngine.Quaternion.AngleAxis(90f, UnityEngine.Vector3.up) *
wheel.WheelAxleCS.ToUnity() / (MathUtil.SIMD_PI * wheel.WheelsRadius)).ToBullet();
float speed = Vector3.Dot(localVelocity, forwardAxis);
wheel.Speed = speed;
if (wheel.EngineForce != 0.0f)
{
//apply torque curves
float engineForce = wheel.EngineForce;
if (speed * engineForce > 0)
{
engineForce *= 1 - (Math.Abs(speed) / MaxWheelAngularVelocity);
}
rollingFriction = engineForce * timeStep;
if (!RootRigidBody.IsActive)
{
RootRigidBody.Activate();
}
}
else
{
float defaultRollingFrictionImpulse = 0.0f;
float maxImpulse = (wheel.Brake != 0) ? wheel.Brake : defaultRollingFrictionImpulse;
rollingFriction = CalcRollingFriction(RootRigidBody, groundObject, wheel.RaycastInfo.ContactPointWS, forwardWS[i], maxImpulse);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
forwardImpulse[i] = 0;
wheelInfo[i].SkidInfo = 1.0f;
if (groundObject != null)
{
wheelInfo[i].SkidInfo = 1.0f;
float maximp = wheel.WheelsSuspensionForce * timeStep * wheel.FrictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
forwardImpulse[i] = rollingFriction;
float x = forwardImpulse[i] * fwdFactor;
float y = sideImpulse[i] * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = maximp / (float)System.Math.Sqrt(impulseSquared);
wheelInfo[i].SkidInfo *= factor;
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < NumWheels; wheel++)
{
if (sideImpulse[wheel] != 0)
{
if (wheelInfo[wheel].SkidInfo < 1.0f)
{
forwardImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
sideImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
}
}
}
}
// apply the impulses
for (int i = 0; i < NumWheels; i++)
{
WheelInfo wheel = wheelInfo[i];
Vector3 rel_pos = wheel.RaycastInfo.ContactPointWS -
RigidBody.CenterOfMassPosition;
if (forwardImpulse[i] != 0)
{
RigidBody.ApplyImpulse(forwardWS[i] * forwardImpulse[i], rel_pos);
}
if (sideImpulse[i] != 0)
{
RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
Vector3 rel_pos2 = wheel.RaycastInfo.ContactPointWS -
groundObject.CenterOfMassPosition;
Vector3 sideImp = axle[i] * sideImpulse[i];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
//Vector4 vChassisWorldUp = RigidBody.CenterOfMassTransform.get_Columns(indexUpAxis);
Vector3 vChassisWorldUp = new Vector3(
RigidBody.CenterOfMassTransform.Row1[indexUpAxis],
RigidBody.CenterOfMassTransform.Row2[indexUpAxis],
RigidBody.CenterOfMassTransform.Row3[indexUpAxis]);
float dot;
Vector3.Dot(ref vChassisWorldUp, ref rel_pos, out dot);
rel_pos -= vChassisWorldUp * (dot * (1.0f - wheel.RollInfluence));
#else
rel_pos[indexUpAxis] *= wheel.RollInfluence;
#endif
RigidBody.ApplyImpulse(sideImp, rel_pos);
//apply friction impulse on the ground
groundObject.ApplyImpulse(-sideImp, rel_pos2);
}
}
}
/// <summary>
/// Get the wheel speed to be used for encoder calculations
/// </summary>
/// <returns></returns>
public float GetWheelSpeed()
{
RobotWheelInfo wheel = robot.RaycastRobot.GetWheelInfo(wheelIndex);
return(wheel.Speed);
}
