protected override JobHandle OnUpdate(JobHandle inputDeps)
{
var physicsWorldSystem = Unity.Entities.World.Active.GetExistingSystem <Unity.Physics.Systems.BuildPhysicsWorld>();
var collisionWorld = physicsWorldSystem.PhysicsWorld.CollisionWorld;
var mousePostion = Input.mousePosition;
var unityRay = Camera.main.ScreenPointToRay(mousePostion);
var ray = new Unity.Physics.Ray(unityRay.origin, unityRay.direction * 10000);
Unity.Physics.RaycastInput input = new Unity.Physics.RaycastInput()
{
Ray = ray,
Filter = new CollisionFilter()
{
CategoryBits = ~0u,
MaskBits = ~0u,
GroupIndex = 0
}
};
Unity.Physics.RaycastHit hit = new Unity.Physics.RaycastHit();
SingleRayCast(collisionWorld, input, ref hit);
bool haveHit = collisionWorld.CastRay(input, out hit);
if (haveHit)
{
mousePostion = new float3(hit.Position.x, hit.Position.y, hit.Position.z);
}
//Debug.Log("Mouse Positon: " + mousePostion);
var job = new MouseInputJob
{
leftClick = Input.GetMouseButtonDown(0),
rightClick = Input.GetMouseButtonDown(1),
mousePosition = mousePostion
};
return(job.Schedule(this, inputDeps));
}
private bool GetPointInWorldFromMousePosition(out float3 pointInWorld)
{
CollisionWorld collisionWorld = m_BuildPhysicsWorldSystem.PhysicsWorld.CollisionWorld;
// Create a new Ray from the camera screen
Vector2 mousePosition = Input.mousePosition;
UnityEngine.Ray unityRay = Camera.main.ScreenPointToRay(mousePosition);
var ray = new Ray(unityRay.origin, unityRay.direction * k_MaxDistance);
var fraction = 1.0f;
RigidBody?hitBody = null;
// Now cast the ray and see if the it hits any colliders.
var rayCastInput = new RaycastInput
{
Ray = ray, Filter = new CollisionFilter
{
CategoryBits = ~0u << 2, // Belongs to group Raycasts
MaskBits = ~0u << 2 // Only collides with Raycasts group
}
};
if (collisionWorld.CastRay(rayCastInput, out RaycastHit hit))
{
hitBody = collisionWorld.Bodies[hit.RigidBodyIndex];
fraction = hit.Fraction;
}
// If we have a hit, then we'll update the singleton data with
// the x and z values. (we're not doing anything with y at the moment)
if (hitBody != null)
{
pointInWorld = ray.Origin + ray.Direction * fraction;
return(true);
}
// default value
pointInWorld = float3.zero;
return(false);
}
// 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;
float3 rayDir = rayEnd - rayStart;
if (mechanics.drawDebugInformation)
{
Debug.DrawRay(rayStart, rayDir);
}
rayInputs[i] = new RaycastInput
{
Ray = new Ray {
Origin = rayStart, Direction = rayDir
},
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)
{
Ray ray = rayInputs[i].Ray;
float3 wheelPos = math.lerp(ray.Origin, (ray.Origin + ray.Direction), rayResult.Fraction);
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();
});
}
