public unsafe Unity.Entities.Entity CreateBody(float3 position, quaternion orientation,
BlobAssetReference <Unity.Physics.Collider> collider,
float3 linearVelocity, float3 angularVelocity, float mass, bool isDynamic)
{
ComponentType[] componentTypes = new ComponentType[isDynamic ? 7 : 3];
componentTypes[0] = typeof(TranslationProxy);
componentTypes[1] = typeof(RotationProxy);
componentTypes[2] = typeof(PhysicsCollider);
if (isDynamic)
{
componentTypes[3] = typeof(PhysicsVelocity);
componentTypes[4] = typeof(PhysicsMass);
componentTypes[5] = typeof(PhysicsDamping);
componentTypes[6] = typeof(PhysicsGravityFactor);
}
Unity.Entities.Entity entity = entityManager.CreateEntity(componentTypes);
entityManager.AddComponentData(entity, new Translation {
Value = position
});
entityManager.AddComponentData(entity, new Rotation {
Value = orientation
});
entityManager.SetComponentData(entity, new PhysicsCollider {
Value = collider
});
if (isDynamic)
{
Unity.Physics.Collider *colliderPtr = (Unity.Physics.Collider *)collider.GetUnsafePtr();
entityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass));
float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity);
entityManager.SetComponentData(entity, new PhysicsVelocity()
{
Linear = linearVelocity,
Angular = angularVelocityLocal
});
entityManager.SetComponentData(entity, new PhysicsDamping()
{
Linear = 1.5f,
Angular = 1.5f
});
entityManager.SetComponentData(entity, new PhysicsGravityFactor {
Value = 0.0f
});
}
return(entity);
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Unity.Physics.Collider *collider,
ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents = default, NativeList <StatefulTriggerEvent> triggerEvents = default)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
// Do a collider cast
{
float3 displacement = newVelocity * remainingTime;
NativeList <ColliderCastHit> triggerHits = default;
if (triggerEvents.IsCreated)
{
triggerHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity / 4, Allocator.Temp);
}
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> collector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world, triggerHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, math.dot(-hit.SurfaceNormal, hit.Fraction * displacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Update trigger events
if (triggerEvents.IsCreated)
{
UpdateTriggersSeen(stepInput, triggerHits, triggerEvents, collector.MinHitFraction);
}
}
// Then do a collider distance for penetration recovery,
// but only fix up penetrating hits
{
// Collider distance query
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <DistanceHit> distanceHitsCollector = new CharacterControllerAllHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits, world);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = stepInput.ContactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
}
// Iterate over penetrating hits and fix up distance and normal
int numConstraints = constraints.Length;
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
if (hit.Distance < stepInput.SkinWidth)
{
bool found = false;
// Iterate backwards to locate the original constraint before the max slope constraint
for (int constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
// Fix up the constraint (normal, distance)
{
// Create new constraint
CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, out SurfaceConstraintInfo newConstraint);
// Resolve its penetration
ResolveConstraintPenetration(ref newConstraint);
// Write back
constraints[constraintIndex] = newConstraint;
}
found = true;
break;
}
}
// Add penetrating hit not caught by collider cast
if (!found)
{
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
}
}
}
// Min delta time for solver to break
float minDeltaTime = 0.0f;
if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
{
// Min delta time to travel at least 1cm
minDeltaTime = 0.01f / math.length(newVelocity);
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies and store collision events
if (affectBodies || collisionEvents.IsCreated)
{
CalculateAndStoreDeferredImpulsesAndCollisionEvents(stepInput, affectBodies, characterMass,
prevVelocity, constraints, ref deferredImpulseWriter, collisionEvents);
}
// Calculate new displacement
float3 newDisplacement = newPosition - prevPosition;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
{
// Check if we can walk to the position simplex solver has suggested
var newCollector = new CharacterControllerClosestHitCollector <ColliderCastHit>(constraints, world, stepInput.RigidBodyIndex, 1.0f);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + newDisplacement
};
world.CastCollider(input, ref newCollector);
if (newCollector.NumHits > 0)
{
ColliderCastHit hit = newCollector.ClosestHit;
// Move character along the newDisplacement direction until it reaches this new contact
{
Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f);
integratedTime *= hit.Fraction;
newPosition = prevPosition + newDisplacement * hit.Fraction;
}
}
}
// Reduce remaining time
remainingTime -= integratedTime;
// Write back position so that the distance query will update results
transform.pos = newPosition;
}
// Write back final velocity
linearVelocity = newVelocity;
}
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,
unsafe void Start()
{
em = World.Active.EntityManager;
CreateArchetypes();
float3[] vertices = new float3[snakeHeadMesh.vertices.Length];
for (int i = 0; i < snakeHeadMesh.vertices.Length; i++)
{
vertices[i] = snakeHeadMesh.vertices[i];
}
float3[] bodyVertices = new float3[snakeBodyMesh.vertices.Length];
for (int i = 0; i < snakeBodyMesh.vertices.Length; i++)
{
bodyVertices[i] = snakeBodyMesh.vertices[i];
}
var CollidesWith = ~0u;
var belongsTo = 1u << 1;
BlobAssetReference <Unity.Physics.Collider> snakeHeadCollider = Unity.Physics.MeshCollider.Create(vertices, snakeHeadMesh.triangles, new CollisionFilter()
{
BelongsTo = belongsTo, CollidesWith = CollidesWith, GroupIndex = 0
});
BlobAssetReference <Unity.Physics.Collider> snakeBodyCollider = Unity.Physics.MeshCollider.Create(vertices, snakeHeadMesh.triangles, new CollisionFilter()
{
BelongsTo = belongsTo, CollidesWith = CollidesWith, GroupIndex = 0
});
float3 startPos = transform.position;
var snakeEntity = em.CreateEntity(snakeHeadArchetype);
var snakePos = startPos;
var snakeRotation = quaternion.identity;
em.SetComponentData(snakeEntity, new SnakeHead()
{
size = 1.0f
});
em.SetComponentData(snakeEntity, new Translation()
{
Value = snakePos
});
em.SetComponentData(snakeEntity, new Rotation()
{
Value = snakeRotation
});
em.SetComponentData(snakeEntity, new LocalToWorld()
{
});
em.SetComponentData(snakeEntity, new MovementSpeed()
{
value = 4.0f
});
em.SetComponentData(snakeEntity, new PhysicsCollider()
{
Value = snakeBodyCollider
});
Unity.Physics.Collider *colliderPtr = (Unity.Physics.Collider *)snakeBodyCollider.GetUnsafePtr();
em.SetComponentData(snakeEntity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass));
float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity);
em.SetComponentData(snakeEntity, new PhysicsVelocity()
{
Linear = linearVelocity, Angular = angularVelocityLocal
});
em.SetComponentData(snakeEntity, new PhysicsDamping()
{
Linear = linearDamping, Angular = angularDamping
});
em.SetSharedComponentData(snakeEntity, new RenderMesh()
{
mesh = snakeHeadMesh, material = snakeHeadMaterial, castShadows = UnityEngine.Rendering.ShadowCastingMode.On, receiveShadows = true
});
var buffer = em.AddBuffer <SnakeBodyBuffer>(snakeEntity);
for (int i = 0; i < nSize; i++)
{
var snakeBodyPos = startPos;
var snakeBodyRotation = quaternion.identity;
buffer.Add(new SnakeBodyBuffer()
{
position = snakeBodyPos, rotation = snakeBodyRotation
});
}
for (int i = 0; i < nSize; i++)
{
var snakeBodyEntity = em.CreateEntity(snakeBodyArchetype);
var snakeBodyPos = startPos;
var snakeBodyRotation = quaternion.identity;
em.SetComponentData(snakeBodyEntity, new SnakeBody()
{
index = i, entity = snakeEntity, size = 1.0f, target = snakeBodyPos
});
em.SetComponentData(snakeBodyEntity, new Translation()
{
Value = snakeBodyPos
});
em.SetComponentData(snakeBodyEntity, new Rotation()
{
Value = snakeBodyRotation
});
em.SetComponentData(snakeBodyEntity, new LocalToWorld()
{
});
em.SetComponentData(snakeBodyEntity, new PhysicsCollider()
{
Value = snakeBodyCollider
});
Unity.Physics.Collider *colliderPtr2 = (Unity.Physics.Collider *)snakeHeadCollider.GetUnsafePtr();
em.SetComponentData(snakeBodyEntity, PhysicsMass.CreateDynamic(colliderPtr2->MassProperties, mass));
em.SetComponentData(snakeBodyEntity, new PhysicsVelocity()
{
Linear = linearVelocity, Angular = angularVelocityLocal
});
em.SetComponentData(snakeBodyEntity, new PhysicsDamping()
{
Linear = linearDamping, Angular = angularDamping
});
em.SetSharedComponentData(snakeBodyEntity, new RenderMesh()
{
mesh = snakeBodyMesh, material = snakeBodyMaterial, castShadows = UnityEngine.Rendering.ShadowCastingMode.On, receiveShadows = true
});
}
}
public unsafe void SetEntityMass(Unity.Entities.Entity _entity, BlobAssetReference <Unity.Physics.Collider> _collider, float _mass)
{
Unity.Physics.Collider *colliderPtr = (Unity.Physics.Collider *)_collider.GetUnsafePtr();
entityManager.SetComponentData(_entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, _mass));
}
