// Update is called once per frame
protected override void OnUpdate()
{
// Make sure the world has finished building before querying it
CreatePhysicsWorldSystem.FinalJobHandle.Complete();
var em = World.Active.EntityManager;
PhysicsWorld world = CreatePhysicsWorldSystem.PhysicsWorld;
float invDt = 1.0f / Time.fixedDeltaTime;
Entities.ForEach((VehicleMechanics mechanics) =>
{
if (mechanics.wheels.Count == 0)
{
return;
}
Entity ce = mechanics.chassisEntity;
if (ce == Entity.Null)
{
return;
}
int ceIdx = world.GetRigidBodyIndex(ce);
if (-1 == ceIdx || ceIdx >= world.NumDynamicBodies)
{
return;
}
//float ceMass = world.GetMass(ceIdx);
float3 cePosition = em.GetComponentData <Translation>(ce).Value;
quaternion ceRotation = em.GetComponentData <Rotation>(ce).Value;
float3 ceCenterOfMass = world.GetCenterOfMass(ceIdx);
float3 ceUp = math.mul(ceRotation, mechanics.chassisUp);
float3 ceForward = math.mul(ceRotation, mechanics.chassisForward);
float3 ceRight = math.mul(ceRotation, mechanics.chassisRight);
var rayResults = new NativeArray <RaycastHit>(mechanics.wheels.Count, Allocator.TempJob);
var rayVelocities = new NativeArray <float3>(mechanics.wheels.Count, Allocator.TempJob);
// Collect the RayCast results
var rayInputs = new NativeArray <RaycastInput>(mechanics.wheels.Count, Allocator.TempJob);
CollisionFilter filter = world.GetCollisionFilter(ceIdx);
for (int i = 0; i < mechanics.wheels.Count; i++)
{
GameObject weGO = mechanics.wheels[i];
float3 wheelCurrentPos = weGO.transform.position;
float3 rayStart = weGO.transform.parent.position;
float3 rayEnd = (-ceUp * (mechanics.suspensionLength + mechanics.wheelBase)) + rayStart;
if (mechanics.drawDebugInformation)
{
Debug.DrawRay(rayStart, rayEnd - rayStart);
}
rayInputs[i] = new RaycastInput
{
Start = rayStart,
End = rayEnd,
Filter = filter
};
}
JobHandle rayJobHandle = ScheduleBatchRayCast(world.CollisionWorld, rayInputs, rayResults);
rayJobHandle.Complete();
for (int i = 0; i < mechanics.wheels.Count; i++)
{
RaycastHit rayResult = rayResults[i];
rayVelocities[i] = float3.zero;
if (rayResult.RigidBodyIndex != -1)
{
float3 wheelPos = rayResult.Position;
wheelPos -= (cePosition - ceCenterOfMass);
float3 velocityAtWheel = world.GetLinearVelocity(ceIdx, wheelPos);
rayVelocities[i] = velocityAtWheel;
}
}
rayInputs.Dispose();
// Calculate a simple slip factor based on chassis tilt.
float slopeSlipFactor = math.pow(math.abs(math.dot(ceUp, math.up())), 4.0f);
// Proportional apply velocity changes to each wheel
float invWheelCount = 1.0f / mechanics.wheels.Count;
for (int i = 0; i < mechanics.wheels.Count; i++)
{
GameObject weGO = mechanics.wheels[i];
float3 rayStart = weGO.transform.parent.position;
float3 rayEnd = (-ceUp * (mechanics.suspensionLength + mechanics.wheelBase)) + rayStart;
float3 rayDir = rayEnd - rayStart;
RaycastHit rayResult = rayResults[i];
//float3 velocityAtWheel = rayVelocities[i];
float3 wheelPos = rayResult.Position;
wheelPos -= (cePosition - ceCenterOfMass);
float3 velocityAtWheel = world.GetLinearVelocity(ceIdx, wheelPos);
float3 weUp = ceUp;
float3 weRight = ceRight;
float3 weForward = ceForward;
#region handle wheel steering
{
bool bIsSteeringWheel = mechanics.steeringWheels.Contains(weGO);
if (bIsSteeringWheel)
{
float steeringAngle = math.radians(mechanics.steeringAngle);
//if((mechanics.steeringWheels.IndexOf(weGO)+1) > (0.5f * mechanics.steeringWheels.Count))
// steeringAngle = -steeringAngle;
quaternion wRotation = quaternion.AxisAngle(ceUp, steeringAngle);
weRight = math.rotate(wRotation, weRight);
weForward = math.rotate(wRotation, weForward);
weGO.transform.localRotation = quaternion.AxisAngle(mechanics.chassisUp, steeringAngle);
}
}
#endregion
float currentSpeedUp = math.dot(velocityAtWheel, weUp);
float currentSpeedForward = math.dot(velocityAtWheel, weForward);
float currentSpeedRight = math.dot(velocityAtWheel, weRight);
#region handle wheel rotation
{
var rGO = weGO.transform.GetChild(0);
if (rGO)
{
bool isDriven = (mechanics.driveEngaged && mechanics.driveWheels.Contains(weGO));
float weRotation = isDriven
? (mechanics.driveDesiredSpeed / mechanics.wheelBase)
: (currentSpeedForward / mechanics.wheelBase);
weRotation = math.radians(weRotation);
rGO.transform.localRotation *= quaternion.AxisAngle(mechanics.chassisRight, weRotation);
}
}
#endregion
float3 wheelCurrentPos = weGO.transform.position;
bool hit = !math.all(rayResult.SurfaceNormal == float3.zero);
if (!hit)
{
float3 wheelDesiredPos = (-ceUp * mechanics.suspensionLength) + rayStart;
weGO.transform.position = math.lerp(wheelCurrentPos, wheelDesiredPos, mechanics.suspensionDamping / mechanics.suspensionStrength);
}
else
{
// remove the wheelbase to get wheel position.
float fraction = rayResult.Fraction - (mechanics.wheelBase) / (mechanics.suspensionLength + mechanics.wheelBase);
float3 wheelDesiredPos = math.lerp(rayStart, rayEnd, fraction);
weGO.transform.position = math.lerp(wheelCurrentPos, wheelDesiredPos, mechanics.suspensionDamping / mechanics.suspensionStrength);
#region Suspension
{
// Calculate and apply the impulses
var posA = rayEnd;
var posB = rayResult.Position;
var lvA = currentSpeedUp * weUp;// world.GetLinearVelocity(ceIdx, posA);
var lvB = world.GetLinearVelocity(rayResult.RigidBodyIndex, posB);
var impulse = mechanics.suspensionStrength * (posB - posA) + mechanics.suspensionDamping * (lvB - lvA);
impulse = impulse * invWheelCount;
float impulseUp = math.dot(impulse, weUp);
// Suspension shouldn't necessarily pull the vehicle down!
float downForceLimit = -0.25f;
if (downForceLimit < impulseUp)
{
impulse = impulseUp * weUp;
world.ApplyImpulse(ceIdx, impulse, posA);
//world.ApplyImpulse(rayResult.RigidBodyIndex, -impulse, posB);
if (mechanics.drawDebugInformation)
{
Debug.DrawRay(wheelDesiredPos, impulse, Color.green);
}
}
}
#endregion
#region Sideways friction
{
float deltaSpeedRight = (0.0f - currentSpeedRight);
deltaSpeedRight = math.clamp(deltaSpeedRight, -mechanics.wheelMaxImpulseRight, mechanics.wheelMaxImpulseRight);
deltaSpeedRight *= mechanics.wheelFrictionRight;
deltaSpeedRight *= slopeSlipFactor;
float3 impulse = deltaSpeedRight * weRight;
float effectiveMass = world.GetEffectiveMass(ceIdx, impulse, wheelPos);
impulse = impulse * effectiveMass * invWheelCount;
world.ApplyImpulse(ceIdx, impulse, wheelPos);
world.ApplyImpulse(rayResult.RigidBodyIndex, -impulse, wheelPos);
if (mechanics.drawDebugInformation)
{
Debug.DrawRay(wheelDesiredPos, impulse, Color.red);
}
}
#endregion
#region Drive
{
if (mechanics.driveEngaged && mechanics.driveWheels.Contains(weGO))
{
float deltaSpeedForward = (mechanics.driveDesiredSpeed - currentSpeedForward);
deltaSpeedForward = math.clamp(deltaSpeedForward, -mechanics.wheelMaxImpulseForward, mechanics.wheelMaxImpulseForward);
deltaSpeedForward *= mechanics.wheelFrictionForward;
deltaSpeedForward *= slopeSlipFactor;
float3 impulse = deltaSpeedForward * weForward;
float effectiveMass = world.GetEffectiveMass(ceIdx, impulse, wheelPos);
impulse = impulse * effectiveMass * invWheelCount;
world.ApplyImpulse(ceIdx, impulse, wheelPos);
world.ApplyImpulse(rayResult.RigidBodyIndex, -impulse, wheelPos);
if (mechanics.drawDebugInformation)
{
Debug.DrawRay(wheelDesiredPos, impulse, Color.blue);
}
}
}
#endregion
}
}
rayResults.Dispose();
rayVelocities.Dispose();
});
}
protected override void OnUpdate()
{
_createPhysicsWorldSystem.GetOutputDependency().Complete();
PhysicsWorld world = _createPhysicsWorldSystem.PhysicsWorld;
var entitys = this.carEntityQuery.ToEntityArrayAsync(Allocator.TempJob, out var jobhandle);
var wheelInfos = new NativeArray <tmpData>(entitys.Length, Allocator.TempJob);
var wheelJob = new WheelJob()
{
physicsWorld = world,
Entitys = entitys,
wbcs = GetComponentDataFromEntity <WheelBaseConfig>(),
pps = GetComponentDataFromEntity <PreviousParent>(),
localToWorlds = GetComponentDataFromEntity <LocalToWorld>(),
wbis = GetComponentDataFromEntity <WheelBaseInfo>(),
time = Time.fixedDeltaTime,
TmpDatas = wheelInfos
};
wheelJob.Schedule(entitys.Length, 80, jobhandle).Complete();
var drift = 0f;
for (int i = 0; i < wheelInfos.Length; i++)
{
var data = wheelInfos[i];
if (data.entity != Entity.Null)
{
EntityManager.SetComponentData(data.entity, data.wbi);
world.ApplyImpulse(data.parentID, data.wbi.SuspensionForce, data.LocalToWorld.Position);
var v = Input.GetAxis("Vertical");
world.ApplyImpulse(data.parentID, data.LocalToWorld.Forward * (30) * v, data.hit.Position + (data.LocalToWorld.Position - data.hit.Position) / 10F);
var physicsVelocity = EntityManager.GetComponentData <PhysicsVelocity>(data.parentEntity);
var position = EntityManager.GetComponentData <Translation>(data.parentEntity);
var rotation = EntityManager.GetComponentData <Rotation>(data.parentEntity);
var h = Input.GetAxis("Horizontal");
world.ApplyAngularForce(data.parentID, h * data.LocalToWorld.Up * 7000);
var tmp = Matrix4x4.identity;
tmp.SetTRS(position.Value, rotation.Value, Vector3.one);
var linearVelocity = world.GetLinearVelocity(data.parentID);
var angularVelocity = world.GetAngularVelocity(data.parentID);
float3 localAngleVelocity = tmp.inverse.MultiplyVector(angularVelocity);
localAngleVelocity.y *= 0.9f + (drift / 10);
physicsVelocity.Angular = tmp.MultiplyVector(localAngleVelocity);
Vector3 localVelocity = tmp.inverse.MultiplyVector(linearVelocity);
localVelocity.x *= 0.9f + (drift / 10);
physicsVelocity.Linear = tmp.MultiplyVector(localVelocity);
world.SetAngularVelocity(data.parentID, physicsVelocity.Angular);
world.SetLinearVelocity(data.parentID, physicsVelocity.Linear);
}
}
entitys.Dispose();
wheelInfos.Dispose();
}
