static float GetInvMassAtPoint(float3 point, float3 normal, RigidBody body, MotionVelocity mv)
{
var massCenter =
math.transform(body.WorldFromBody, body.Collider.Value.MassProperties.MassDistribution.Transform.pos);
float3 arm = point - massCenter;
float3 jacAng = math.cross(arm, normal);
float3 armC = jacAng * mv.InverseInertia;
float objectMassInv = math.dot(armC, jacAng);
objectMassInv += mv.InverseMass;
return(objectMassInv);
}
public void MotionVelocityCalculateExpansionTest()
{
var motionVelocity = new MotionVelocity()
{
LinearVelocity = new float3(2.0f, 1.0f, 5.0f),
AngularVelocity = new float3(3.0f, 4.0f, 5.0f),
InverseInertiaAndMass = new float4(2.0f, 3.0f, 4.0f, 2.0f),
AngularExpansionFactor = 1.2f
};
var motionExpansion = motionVelocity.CalculateExpansion(1.0f / 60.0f);
Assert.AreEqual(new float3(1.0f / 30.0f, 1.0f / 60.0f, 1.0f / 12.0f), motionExpansion.Linear);
Assert.AreApproximatelyEqual((float)math.SQRT2 / 10.0f, motionExpansion.Uniform);
}
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter)
{
bool bothBodiesWithInfInertiaAndMass = velocityA.HasInfiniteInertiaAndMass && velocityB.HasInfiniteInertiaAndMass;
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
}
}
/// <summary>
/// Add to the angular velocity of a rigid body considering deltaTime (in world space)
/// </summary>
/// <param name="world"></param>
/// <param name="rigidBodyIndex"></param>
/// <param name="angularVelocity"></param>
public static void ApplyAngularAcceleration(this PhysicsWorld world, int rigidBodyIndex, float3 angularVelocity)
{
if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies))
{
return;
}
MotionData md = world.MotionDatas[rigidBodyIndex];
float3 angularVelocityMotionSpace = math.rotate(math.inverse(md.WorldFromMotion.rot), angularVelocity);
Unity.Collections.NativeSlice <MotionVelocity> motionVelocities = world.MotionVelocities;
MotionVelocity mv = motionVelocities[rigidBodyIndex];
mv.AngularVelocity += angularVelocityMotionSpace * Time.deltaTime;
motionVelocities[rigidBodyIndex] = mv;
}
// Solve the Jacobian
public void SolveContact(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter)
{
// Copy velocity data
MotionVelocity tempVelocityA = velocityA;
MotionVelocity tempVelocityB = velocityB;
if (jacHeader.HasMassFactors)
{
MassFactors jacMod = jacHeader.AccessMassFactors();
tempVelocityA.InverseInertia *= jacMod.InverseInertiaFactorA;
tempVelocityA.InverseMass *= jacMod.InverseMassFactorA;
tempVelocityB.InverseInertia *= jacMod.InverseInertiaFactorB;
tempVelocityB.InverseMass *= jacMod.InverseMassFactorB;
}
// Solve normal impulses
float sumImpulses = 0.0f;
float totalAccumulatedImpulse = 0.0f;
bool forceCollisionEvent = false;
for (int j = 0; j < BaseJacobian.NumContacts; j++)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j);
// Solve velocity so that predicted contact distance is greater than or equal to zero
float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac, tempVelocityA, tempVelocityB);
float dv = jacAngular.VelToReachCp - relativeVelocity;
float impulse = dv * jacAngular.Jac.EffectiveMass;
float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse;
ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB);
}
jacAngular.Jac.Impulse = accumulatedImpulse;
sumImpulses += accumulatedImpulse;
totalAccumulatedImpulse += jacAngular.Jac.Impulse;
// Force contact event even when no impulse is applied, but there is penetration.
forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f;
}
/// <summary>
/// Apply an Acceleration to a rigid body at a point considering mass (in world space)
/// </summary>
/// <param name="world"></param>
/// <param name="rigidBodyIndex"></param>
/// <param name="linearImpulse"></param>
/// <param name="point"></param>
public static void ApplyAccelerationImpulse(this PhysicsWorld world, int rigidBodyIndex, float3 linearImpulse, float3 point)
{
if (!(0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies))
{
return;
}
MotionData md = world.MotionDatas[rigidBodyIndex];
float3 angularImpulseWorldSpace = math.cross(point - md.WorldFromMotion.pos, linearImpulse);
float3 angularImpulseMotionSpace = math.rotate(math.inverse(md.WorldFromMotion.rot), angularImpulseWorldSpace);
Unity.Collections.NativeSlice <MotionVelocity> motionVelocities = world.MotionVelocities;
MotionVelocity mv = motionVelocities[rigidBodyIndex];
mv.LinearVelocity += (linearImpulse) * Time.deltaTime;
mv.AngularVelocity += (angularImpulseMotionSpace) * Time.deltaTime;
motionVelocities[rigidBodyIndex] = mv;
}
private static unsafe void ResolveContacts(PhysicsWorld world, float deltaTime, float3 gravity, float tau, float damping,
float characterMass, float3 linearVelocity, int numConstraints, ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter)
{
for (int i = 0; i < numConstraints; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / deltaTime) * tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
float3 impulse = float3.zero;
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float3 arm = constraint.HitPosition - (body.WorldFromBody.pos + body.Collider->MassProperties.MassDistribution.Transform.pos);
float3 jacAng = math.cross(arm, constraint.Plane.Normal);
float3 armC = jacAng * mv.InverseInertiaAndMass.xyz;
float objectMassInv = math.dot(armC, jacAng);
objectMassInv += mv.InverseInertiaAndMass.w;
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = gravity * deltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
}
// Calculate extra details about the collision, by re-integrating the leaf colliders to the time of collision
internal unsafe CollisionEvent.Details CalculateDetails(
ref PhysicsWorld physicsWorld, float timeStep, Velocity inputVelocityA, Velocity inputVelocityB, NativeArray <ContactPoint> narrowPhaseContactPoints)
{
int bodyIndexA = BodyIndices.BodyIndexA;
int bodyIndexB = BodyIndices.BodyIndexB;
bool bodyAIsDynamic = bodyIndexA < physicsWorld.MotionVelocities.Length;
bool bodyBIsDynamic = bodyIndexB < physicsWorld.MotionVelocities.Length;
MotionVelocity motionVelocityA = bodyAIsDynamic ? physicsWorld.MotionVelocities[bodyIndexA] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = bodyBIsDynamic ? physicsWorld.MotionVelocities[bodyIndexB] : MotionVelocity.Zero;
MotionData motionDataA = bodyAIsDynamic ? physicsWorld.MotionDatas[bodyIndexA] : MotionData.Zero;
MotionData motionDataB = bodyBIsDynamic ? physicsWorld.MotionDatas[bodyIndexB] : MotionData.Zero;
float estimatedImpulse = SolverImpulse;
// First calculate minimum time of impact and estimate the impulse
float toi = timeStep;
{
float sumRemainingVelocities = 0.0f;
float numRemainingVelocities = 0.0f;
for (int i = 0; i < narrowPhaseContactPoints.Length; i++)
{
var cp = narrowPhaseContactPoints[i];
// Collect data for impulse estimation
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion,
motionVelocityA.LinearVelocity, motionVelocityA.AngularVelocity, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion,
motionVelocityB.LinearVelocity, motionVelocityB.AngularVelocity, cp.Position);
float projRelVel = math.dot(pointVelB - pointVelA, Normal);
if (projRelVel > 0.0f)
{
sumRemainingVelocities += projRelVel;
numRemainingVelocities += 1.0f;
}
}
// Get minimum time of impact
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion,
inputVelocityA.Linear, inputVelocityA.Angular, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion,
inputVelocityB.Linear, inputVelocityB.Angular, cp.Position);
float projRelVel = math.dot(pointVelB - pointVelA, Normal);
if (projRelVel > 0.0f)
{
float newToi = math.max(0.0f, cp.Distance / projRelVel);
toi = math.min(toi, newToi);
}
else if (cp.Distance <= 0.0f)
{
// If in penetration, time of impact is 0 for sure
toi = 0.0f;
}
}
}
if (numRemainingVelocities > 0.0f)
{
float sumInvMass = motionVelocityA.InverseMass + motionVelocityB.InverseMass;
estimatedImpulse += sumRemainingVelocities / (numRemainingVelocities * sumInvMass);
}
}
// Then, sub-integrate for time of impact and keep contact points closer than hitDistanceThreshold
int closestContactIndex = -1;
float minDistance = float.MaxValue;
{
int estimatedContactPointCount = 0;
for (int i = 0; i < narrowPhaseContactPoints.Length; i++)
{
// Estimate new position
var cp = narrowPhaseContactPoints[i];
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion, inputVelocityA.Linear, inputVelocityA.Angular, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion, inputVelocityB.Linear, inputVelocityB.Angular, cp.Position);
float3 relVel = pointVelB - pointVelA;
float projRelVel = math.dot(relVel, Normal);
// Only sub integrate if approaching, otherwise leave it as is
// (it can happen that input velocity was separating but there
// still was a collision event - penetration recovery, or other
// body pushing in different direction).
if (projRelVel > 0.0f)
{
// Position the point on body A
cp.Position += Normal * cp.Distance;
// Sub integrate the point
cp.Position -= relVel * toi;
// Reduce the distance
cp.Distance -= projRelVel * toi;
}
// Filter out contacts that are still too far away
if (cp.Distance <= physicsWorld.CollisionWorld.CollisionTolerance)
{
narrowPhaseContactPoints[estimatedContactPointCount++] = cp;
}
else if (cp.Distance < minDistance)
{
minDistance = cp.Distance;
closestContactIndex = i;
}
}
}
// If due to estimation of relative velocity no contact points will
// get closer than the tolerance, we need to export the closest one
// to make sure at least one contact point is reported.
if (estimatedContactPointCount == 0)
{
narrowPhaseContactPoints[estimatedContactPointCount++] = narrowPhaseContactPoints[closestContactIndex];
}
// Instantiate collision details and allocate memory
var details = new CollisionEvent.Details
{
EstimatedContactPointPositions = new NativeArray <float3>(estimatedContactPointCount, Allocator.Temp),
EstimatedImpulse = estimatedImpulse
};
// Fill the contact point positions array
for (int i = 0; i < estimatedContactPointCount; i++)
{
details.EstimatedContactPointPositions[i] = narrowPhaseContactPoints[i].Position;
}
return(details);
}
}
public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
NativeArray <Translation> chunkPositions = chunk.GetNativeArray(PositionType);
NativeArray <Rotation> chunkRotations = chunk.GetNativeArray(RotationType);
NativeArray <PhysicsVelocity> chunkVelocities = chunk.GetNativeArray(PhysicsVelocityType);
NativeArray <PhysicsMass> chunkMasses = chunk.GetNativeArray(PhysicsMassType);
NativeArray <PhysicsDamping> chunkDampings = chunk.GetNativeArray(PhysicsDampingType);
NativeArray <PhysicsGravityFactor> chunkGravityFactors = chunk.GetNativeArray(PhysicsGravityFactorType);
int motionStart = firstEntityIndex;
int instanceCount = chunk.Count;
bool hasChunkPhysicsGravityFactorType = chunk.Has(PhysicsGravityFactorType);
bool hasChunkPhysicsDampingType = chunk.Has(PhysicsDampingType);
bool hasChunkPhysicsMassType = chunk.Has(PhysicsMassType);
// Note: Transform and AngularExpansionFactor could be calculated from PhysicsCollider.MassProperties
// However, to avoid the cost of accessing the collider we assume an infinite mass at the origin of a ~1m^3 box.
// For better performance with spheres, or better behavior for larger and/or more irregular colliders
// you should add a PhysicsMass component to get the true values
var defaultPhysicsMass = new PhysicsMass()
{
Transform = RigidTransform.identity,
InverseMass = 0.0f,
InverseInertia = float3.zero,
AngularExpansionFactor = 1.0f,
};
// Create motion velocities
var defaultInverseInertiaAndMass = new float4(defaultPhysicsMass.InverseInertia, defaultPhysicsMass.InverseMass);
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionVelocities[motionIndex] = new MotionVelocity
{
LinearVelocity = chunkVelocities[i].Linear, // world space
AngularVelocity = chunkVelocities[i].Angular, // inertia space
InverseInertiaAndMass = hasChunkPhysicsMassType ? new float4(chunkMasses[i].InverseInertia, chunkMasses[i].InverseMass) : defaultInverseInertiaAndMass,
AngularExpansionFactor = hasChunkPhysicsMassType ? chunkMasses[i].AngularExpansionFactor : defaultPhysicsMass.AngularExpansionFactor,
};
}
// Note: these defaults assume a dynamic body with infinite mass, hence no damping
var defaultPhysicsDamping = new PhysicsDamping()
{
Linear = 0.0f,
Angular = 0.0f,
};
// Note: if a dynamic body infinite mass then assume no gravity should be applied
float defaultGravityFactor = hasChunkPhysicsMassType ? 1.0f : 0.0f;
// Create motion datas
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionDatas[motionIndex] = CreateMotionData(
chunkPositions[i], chunkRotations[i],
hasChunkPhysicsMassType ? chunkMasses[i] : defaultPhysicsMass,
hasChunkPhysicsDampingType ? chunkDampings[i] : defaultPhysicsDamping,
hasChunkPhysicsGravityFactorType ? chunkGravityFactors[i].Value : defaultGravityFactor);
}
}
private static unsafe void CalculateAndStoreDeferredImpulsesAndCollisionEvents(
CharacterControllerStepInput stepInput, bool affectBodies, float characterMass,
float3 linearVelocity, NativeList <SurfaceConstraintInfo> constraints, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < constraints.Length; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
float3 impulse = float3.zero;
if (rigidBodyIndex < 0)
{
continue;
}
// Skip static bodies if needed to calculate impulse
if (affectBodies && (rigidBodyIndex < world.NumDynamicBodies))
{
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * stepInput.Damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv);
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
if (collisionEvents.IsCreated && constraint.Touched && !constraint.IsMaxSlope)
{
var collisionEvent = new StatefulCollisionEvent(world.Bodies[stepInput.RigidBodyIndex].Entity,
world.Bodies[rigidBodyIndex].Entity, stepInput.RigidBodyIndex, rigidBodyIndex, ColliderKey.Empty,
constraint.ColliderKey, constraint.Plane.Normal);
collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(
1, math.dot(impulse, collisionEvent.Normal), constraint.HitPosition);
// check if collision event exists for the same bodyID and colliderKey
// although this is a nested for, number of solved constraints shouldn't be high
// if the same constraint (same entities, rigidbody indices and collider keys)
// is solved in multiple solver iterations, pick the one from latest iteration
bool newEvent = true;
for (int j = 0; j < collisionEvents.Length; j++)
{
if (collisionEvents[j].CompareTo(collisionEvent) == 0)
{
collisionEvents[j] = collisionEvent;
newEvent = false;
break;
}
}
if (newEvent)
{
collisionEvents.Add(collisionEvent);
}
}
}
}
private static unsafe void CalculateAndStoreDeferredImpulses(
CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity, int numConstraints,
ref NativeArray <SurfaceConstraintInfo> constraints, ref BlockStream.Writer deferredImpulseWriter)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < numConstraints; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * stepInput.Damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
float3 impulse = float3.zero;
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv);
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
}
internal static void CreateRigidBodiesAndMotions(SimulationStepInput input,
NativeList <BodyInfo> bodies, NativeHashMap <int, int> indexMap)
{
NativeArray <RigidBody> dynamicBodies = input.World.DynamicBodies;
NativeArray <RigidBody> staticBodies = input.World.StaticBodies;
NativeArray <MotionData> motionDatas = input.World.MotionDatas;
NativeArray <MotionVelocity> motionVelocities = input.World.MotionVelocities;
int dynamicBodyIndex = 0;
int staticBodyIndex = 0;
for (int i = 0; i < bodies.Length; i++)
{
BodyInfo bodyInfo = bodies[i];
unsafe
{
Unity.Physics.Collider *collider = (Unity.Physics.Collider *)bodyInfo.Collider.GetUnsafePtr();
if (bodyInfo.IsDynamic)
{
dynamicBodies[dynamicBodyIndex] = new RigidBody
{
WorldFromBody = new RigidTransform(bodyInfo.Orientation, bodyInfo.Position),
Collider = bodyInfo.Collider,
Entity = Entity.Null,
CustomTags = 0
};
motionDatas[dynamicBodyIndex] = new MotionData
{
WorldFromMotion = new RigidTransform(
math.mul(bodyInfo.Orientation, collider->MassProperties.MassDistribution.Transform.rot),
math.rotate(bodyInfo.Orientation, collider->MassProperties.MassDistribution.Transform.pos) + bodyInfo.Position
),
BodyFromMotion = new RigidTransform(collider->MassProperties.MassDistribution.Transform.rot, collider->MassProperties.MassDistribution.Transform.pos),
LinearDamping = 0.0f,
AngularDamping = 0.0f
};
motionVelocities[dynamicBodyIndex] = new MotionVelocity
{
LinearVelocity = bodyInfo.LinearVelocity,
AngularVelocity = bodyInfo.AngularVelocity,
InverseInertia = math.rcp(collider->MassProperties.MassDistribution.InertiaTensor * bodyInfo.Mass),
InverseMass = math.rcp(bodyInfo.Mass),
AngularExpansionFactor = collider->MassProperties.AngularExpansionFactor,
GravityFactor = 1.0f
};
indexMap.Add(i, dynamicBodyIndex);
dynamicBodyIndex++;
}
else
{
staticBodies[staticBodyIndex] = new RigidBody
{
WorldFromBody = new RigidTransform(bodyInfo.Orientation, bodyInfo.Position),
Collider = bodyInfo.Collider,
Entity = Entity.Null,
CustomTags = 0
};
staticBodyIndex++;
}
}
}
// Create default static body
unsafe
{
staticBodies[staticBodyIndex] = new RigidBody
{
WorldFromBody = new RigidTransform(quaternion.identity, float3.zero),
Collider = default,
public void CalculateDetailsTest()
{
// Simple collision event with straight normal and 3 contact points
LowLevel.CollisionEvent lowLevelEvent = new LowLevel.CollisionEvent();
lowLevelEvent.BodyIndices.BodyAIndex = 0;
lowLevelEvent.BodyIndices.BodyBIndex = 1;
lowLevelEvent.ColliderKeys.ColliderKeyA = ColliderKey.Empty;
lowLevelEvent.ColliderKeys.ColliderKeyB = ColliderKey.Empty;
lowLevelEvent.Normal = new float3(0.0f, -1.00000f, 0.0f);
lowLevelEvent.NumNarrowPhaseContactPoints = 3;
lowLevelEvent.SolverImpulse = 1.0f;
// Wrapping collision event
CollisionEvent collisionEvent = new CollisionEvent();
collisionEvent.EventData = lowLevelEvent;
collisionEvent.TimeStep = 1.0f / 60.0f;
// Input velocity is obviously separating, but high angular velocity still caused an event
collisionEvent.InputVelocityA = new Velocity
{
Angular = new float3(-0.00064f, 11.17604f, 0.02133f),
Linear = new float3(-3.81205f, -0.56607f, 9.14945f)
};
collisionEvent.InputVelocityB = new Velocity
{
Angular = new float3(0.00000f, 0.00000f, 0.00000f),
Linear = new float3(0.00000f, 0.00000f, 0.00000f)
};
// Initialize 3 contact points
collisionEvent.NarrowPhaseContactPoints = new NativeArray <ContactPoint>(3, Allocator.Temp)
{
[0] = new ContactPoint
{
Distance = 0.177905f,
Position = new float3(-22.744950f, 2.585318f, -50.108990f)
},
[1] = new ContactPoint
{
Distance = 0.276652f,
Position = new float3(-20.731140f, 2.486506f, -50.322240f)
},
[2] = new ContactPoint
{
Distance = 0.278534f,
Position = new float3(-20.766140f, 2.484623f, -50.652630f)
}
};
// Allocate a simple world of 1 dynamic and 1 static body
var simpleWorld = new Physics.PhysicsWorld(1, 1, 0);
var motionVelocities = simpleWorld.MotionVelocities;
var motionDatas = simpleWorld.MotionDatas;
motionDatas[0] = new MotionData
{
LinearDamping = 0.0f,
AngularDamping = 0.0f,
GravityFactor = 1.0f,
BodyFromMotion = new RigidTransform(new quaternion(0.0f, 0.0f, 0.0f, 1.0f), new float3(0.0f, 0.0f, 0.0f)),
WorldFromMotion = new RigidTransform(new quaternion(0.09212853f, 0.1400256f, -0.006776567f, -0.9858292f), new float3(-22.17587f, 0.5172966f, -52.24425f))
};
motionVelocities[0] = new MotionVelocity
{
LinearVelocity = new float3(-3.81221f, -1.37538f, -15.41893f),
AngularVelocity = new float3(-7.30913f, -4.78899f, 1.14168f),
InverseInertiaAndMass = new float4(0.00045f, 0.00045f, 0.00045f, 0.00018f),
AngularExpansionFactor = 2.05061f
};
// Calculate the collision event details and make sure 1 contact point is returned
var details = collisionEvent.CalculateDetails(ref simpleWorld);
Assert.AreEqual(details.EstimatedContactPointPositions.Length, 1);
// Dispose the world data
simpleWorld.Dispose();
}
void generateRandomMotion(ref Random rnd, out MotionVelocity velocity, out MotionData motion, bool allowInfiniteMass)
{
motion = new MotionData
{
WorldFromMotion = generateRandomTransform(ref rnd),
BodyFromMotion = generateRandomTransform(ref rnd)
};
float3 inertia = rnd.NextFloat3(1e-3f, 100.0f);
switch (rnd.NextInt(3))
{
case 0: // all values random
break;
case 1: // two values the same
int index = rnd.NextInt(3);
inertia[(index + 1) % 2] = inertia[index];
break;
case 2: // all values the same
inertia = inertia.zzz;
break;
}
float3 nextLinVel;
if (rnd.NextBool())
{
nextLinVel = float3.zero;
}
else
{
nextLinVel = rnd.NextFloat3(-50.0f, 50.0f);
}
float3 nextAngVel;
if (rnd.NextBool())
{
nextAngVel = float3.zero;
}
else
{
nextAngVel = rnd.NextFloat3(-50.0f, 50.0f);
}
float3 nextInertia;
float nextMass;
if (allowInfiniteMass && rnd.NextBool())
{
nextInertia = float3.zero;
nextMass = 0.0f;
}
else
{
nextMass = rnd.NextFloat(1e-3f, 100.0f);
nextInertia = 1.0f / inertia;
}
velocity = new MotionVelocity
{
LinearVelocity = nextLinVel,
AngularVelocity = nextAngVel,
InverseInertia = nextInertia,
InverseMass = nextMass
};
}
static void integrate(ref MotionVelocity velocity, ref MotionData motion, float timestep)
{
motion.WorldFromMotion.pos += velocity.LinearVelocity * timestep;
Integrator.IntegrateOrientation(ref motion.WorldFromMotion.rot, velocity.AngularVelocity, timestep);
}
// Calculate extra details about the collision, by re-integrating the leaf colliders to the time of collision
internal unsafe Physics.CollisionEvent.Details CalculateDetails(
ref PhysicsWorld physicsWorld, float timeStep, Velocity inputVelocityA, Velocity inputVelocityB, NativeArray <ContactPoint> narrowPhaseContactPoints)
{
int bodyAIndex = BodyIndices.BodyAIndex;
int bodyBIndex = BodyIndices.BodyBIndex;
bool bodyAIsDynamic = bodyAIndex < physicsWorld.MotionVelocities.Length;
bool bodyBIsDynamic = bodyBIndex < physicsWorld.MotionVelocities.Length;
MotionVelocity motionVelocityA = bodyAIsDynamic ? physicsWorld.MotionVelocities[bodyAIndex] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = bodyBIsDynamic ? physicsWorld.MotionVelocities[bodyBIndex] : MotionVelocity.Zero;
MotionData motionDataA = bodyAIsDynamic ? physicsWorld.MotionDatas[bodyAIndex] : MotionData.Zero;
MotionData motionDataB = bodyBIsDynamic ? physicsWorld.MotionDatas[bodyBIndex] : MotionData.Zero;
float estimatedImpulse = SolverImpulse;
// First calculate minimum time of impact and estimate the impulse
float toi = timeStep;
{
float sumRemainingVelocities = 0.0f;
float numRemainingVelocities = 0.0f;
for (int i = 0; i < narrowPhaseContactPoints.Length; i++)
{
var cp = narrowPhaseContactPoints[i];
// Collect data for impulse estimation
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion,
motionVelocityA.LinearVelocity, motionVelocityA.AngularVelocity, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion,
motionVelocityB.LinearVelocity, motionVelocityB.AngularVelocity, cp.Position);
float projRelVel = math.dot(pointVelB - pointVelA, Normal);
if (projRelVel > 0.0f)
{
sumRemainingVelocities += projRelVel;
numRemainingVelocities += 1.0f;
}
}
// Get minimum time of impact
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion,
inputVelocityA.Linear, inputVelocityA.Angular, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion,
inputVelocityB.Linear, inputVelocityB.Angular, cp.Position);
float projRelVel = math.dot(pointVelB - pointVelA, Normal);
if (projRelVel > 0.0f)
{
float newToi = math.max(0.0f, cp.Distance / projRelVel);
toi = math.min(toi, newToi);
}
else if (cp.Distance <= 0.0f)
{
// If in penetration, time of impact is 0 for sure
toi = 0.0f;
}
}
}
if (numRemainingVelocities > 0.0f)
{
float sumInvMass = motionVelocityA.InverseInertiaAndMass.w + motionVelocityB.InverseInertiaAndMass.w;
estimatedImpulse += sumRemainingVelocities / (numRemainingVelocities * sumInvMass);
}
}
// Then, sub-integrate for time of impact and keep contact points closer than hitDistanceThreshold
{
int estimatedContactPointCount = 0;
for (int i = 0; i < narrowPhaseContactPoints.Length; i++)
{
// Estimate new position
var cp = narrowPhaseContactPoints[i];
{
float3 pointVelA = GetPointVelocity(motionDataA.WorldFromMotion, inputVelocityA.Linear, inputVelocityA.Angular, cp.Position + Normal * cp.Distance);
float3 pointVelB = GetPointVelocity(motionDataB.WorldFromMotion, inputVelocityB.Linear, inputVelocityB.Angular, cp.Position);
float3 relVel = pointVelB - pointVelA;
float projRelVel = math.dot(relVel, Normal);
// Position the point on body A
cp.Position += Normal * cp.Distance;
// Sub integrate the point
cp.Position -= relVel * toi;
// Reduce the distance
cp.Distance -= projRelVel * toi;
// Filter out contacts that are still too far away
if (cp.Distance <= physicsWorld.CollisionWorld.CollisionTolerance)
{
narrowPhaseContactPoints[estimatedContactPointCount++] = cp;
}
}
}
// Instantiate collision details and allocate memory
var details = new Physics.CollisionEvent.Details
{
EstimatedContactPointPositions = new NativeArray <float3>(estimatedContactPointCount, Allocator.Temp),
EstimatedImpulse = estimatedImpulse
};
// Fill the contact point positions array
for (int i = 0; i < estimatedContactPointCount; i++)
{
details.EstimatedContactPointPositions[i] = narrowPhaseContactPoints[i].Position;
}
return(details);
}
}
internal static float GetJacVelocity(float3 linear, ContactJacobianAngular jacAngular, MotionVelocity velocityA, MotionVelocity velocityB)
{
float3 temp = (velocityA.LinearVelocity - velocityB.LinearVelocity) * linear;
temp += velocityA.AngularVelocity * jacAngular.AngularA;
temp += velocityB.AngularVelocity * jacAngular.AngularB;
return(math.csum(temp));
}
public unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
var chunkPositions = chunk.GetNativeArray(PositionType);
var chunkRotations = chunk.GetNativeArray(RotationType);
var chunkVelocities = chunk.GetNativeArray(PhysicsVelocityType);
var chunkMasses = chunk.GetNativeArray(PhysicsMassType);
int motionStart = firstEntityIndex;
int instanceCount = chunk.Count;
// Create motion velocities
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionVelocities[motionIndex] = new MotionVelocity
{
LinearVelocity = chunkVelocities[i].Linear, // world space
AngularVelocity = chunkVelocities[i].Angular, // inertia space
InverseInertiaAndMass = new float4(chunkMasses[i].InverseInertia, chunkMasses[i].InverseMass),
AngularExpansionFactor = chunkMasses[i].AngularExpansionFactor
};
}
// Create motion datas
const float defaultLinearDamping = 0.0f; // TODO: Use non-zero defaults?
const float defaultAngularDamping = 0.0f;
if (chunk.Has(PhysicsGravityFactorType))
{
// With gravity factor ...
var chunkGravityFactors = chunk.GetNativeArray(PhysicsGravityFactorType);
if (chunk.Has(PhysicsDampingType))
{
// ... with damping
var chunkDampings = chunk.GetNativeArray(PhysicsDampingType);
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionDatas[motionIndex] = CreateMotionData(
chunkPositions[i], chunkRotations[i], chunkMasses[i],
chunkDampings[i].Linear, chunkDampings[i].Angular, chunkGravityFactors[i].Value);
}
}
else
{
// ... without damping
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionDatas[motionIndex] = CreateMotionData(
chunkPositions[i], chunkRotations[i], chunkMasses[i],
defaultLinearDamping, defaultAngularDamping, chunkGravityFactors[i].Value);
}
}
}
else
{
// Without gravity factor ...
if (chunk.Has(PhysicsDampingType))
{
// ... with damping
var chunkDampings = chunk.GetNativeArray(PhysicsDampingType);
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionDatas[motionIndex] = CreateMotionData(
chunkPositions[i], chunkRotations[i], chunkMasses[i],
chunkDampings[i].Linear, chunkDampings[i].Angular);
}
}
else
{
// ... without damping
for (int i = 0, motionIndex = motionStart; i < instanceCount; i++, motionIndex++)
{
MotionDatas[motionIndex] = CreateMotionData(
chunkPositions[i], chunkRotations[i], chunkMasses[i],
defaultLinearDamping, defaultAngularDamping);
}
}
}
}
