using System;
using System.IO;
using Unity.Burst;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Entities;
using Unity.Jobs;
using Unity.Mathematics;
using Unity.Physics;
using Unity.Physics.Extensions;
using Unity.Transforms;
using Collider = Unity.Physics.Collider;
namespace GarmentButton.VerletIntegration.Interaction
{
[UpdateInGroup(typeof(VerletIntegrationPhysicsGroupSystems))]
[UpdateBefore(typeof(SetActiveStateInteractionFlagSystem))]
[BurstCompile]
public partial struct SimulateInteractionSystem : ISystem
{
[BurstCompile]
public void OnCreate(ref SystemState state)
{
state.RequireForUpdate<Point>();
state.RequireForUpdate<FlagInteractionEnableTag>();
state.RequireForUpdate<InteractableWithClothTag>();
state.RequireForUpdate<LocalTransform>();
}
//[BurstCompile]
public void OnUpdate(ref SystemState state)
{
var physicsColliderLookup = SystemAPI.GetComponentLookup<PhysicsCollider>(true);
var physicsVelocityLookup = SystemAPI.GetComponentLookup<PhysicsVelocity>(true);
var transformLookUp = SystemAPI.GetComponentLookup<LocalToWorld>(true);
var pointBuffer = SystemAPI.GetBufferLookup<Point>();
foreach (var (currentInteractable, pointLenght, entity) in SystemAPI
.Query<
DynamicBuffer<CurrentInteractable>, RefRO<PointArrayLenght>>()
.WithAll<FlagInteractionEnableTag>().WithEntityAccess())
{
var interactionCount = currentInteractable.Length;
if (interactionCount == 0)
continue;
NativeParallelMultiHashMap<byte, ColliderCollection> colliders =
new NativeParallelMultiHashMap<byte, ColliderCollection>(3, Allocator.TempJob);
for (int i = 0; i < interactionCount; i++)
{
var collider = physicsColliderLookup.GetRefRO(currentInteractable[i].Entity);
unsafe
{
var colliderPointer = collider.ValueRO.Value.AsPtr();
var colliderEntity = currentInteractable[i].Entity;
ExtractCollider(colliderPointer, colliders, colliderEntity, float4x4.identity);
}
}
NativeArray<JobHandle> handles =
new NativeArray<JobHandle>(4, Allocator.TempJob);
handles[0] = state.Dependency;
var key = (byte)ColliderType.Capsule;
var count = colliders.CountValuesForKey(key);
NativeArray<CapsuleInteraction> capsuleCollection =
new NativeArray<CapsuleInteraction>(count, Allocator.TempJob);
if (count > 0)
{
var entities = new NativeArray<ColliderCollection>(count, Allocator.TempJob);
int i = 0;
foreach (var e in
colliders.GetValuesForKey(
key))
entities[i++] = e;
handles[1] = new CollectColliderCapsule()
{
Collection = capsuleCollection,
Colliders = entities,
PhysicsVelocityLookup = physicsVelocityLookup,
LocalToWorldLookup = transformLookUp,
DeltaTime = SystemAPI.Time.fixedDeltaTime,
}.Schedule(count, state.Dependency);
entities.Dispose(handles[1]);
}
key = (byte)ColliderType.Sphere;
count = colliders.CountValuesForKey(key);
NativeArray<SphereInteraction> sphereCollection =
new NativeArray<SphereInteraction>(count, Allocator.TempJob);
if (count > 0)
{
var entities = new NativeArray<ColliderCollection>(count, Allocator.TempJob);
int i = 0;
foreach (var e in
colliders.GetValuesForKey(
key))
entities[i++] = e;
handles[2] = new CollectColliderSphere()
{
Collection = sphereCollection,
Colliders = entities,
PhysicsVelocityLookup = physicsVelocityLookup,
LocalToWorldLookup = transformLookUp,
DeltaTime = SystemAPI.Time.fixedDeltaTime,
}.Schedule(count, state.Dependency);
entities.Dispose(handles[2]);
}
key = (byte)ColliderType.Box;
count = colliders.CountValuesForKey(key);
NativeArray<BoxInteraction> boxCollection =
new NativeArray<BoxInteraction>(count, Allocator.TempJob);
if (count > 0)
{
var entities = new NativeArray<ColliderCollection>(count, Allocator.TempJob);
int i = 0;
foreach (var e in
colliders.GetValuesForKey(
key))
entities[i++] = e;
handles[3] = new CollectColliderBox()
{
Collection = boxCollection,
Colliders = entities,
PhysicsVelocityLookup = physicsVelocityLookup,
LocalToWorldLookup = transformLookUp,
DeltaTime = SystemAPI.Time.fixedDeltaTime,
}.Schedule(count, state.Dependency);
entities.Dispose(handles[3]);
}
JobHandle combined = JobHandle.CombineDependencies(handles);
colliders.Dispose(combined);
state.Dependency = combined;
state.Dependency = new InteractJob()
{
PointSphereRadius = 0.1f,
CapsuleCollection = capsuleCollection.AsReadOnly(),
SphereCollection = sphereCollection.AsReadOnly(),
BoxCollection = boxCollection.AsReadOnly(),
BufferLookup = pointBuffer,
Entity = entity
}.ScheduleParallel(pointLenght.ValueRO.Value, 16, combined);
capsuleCollection.Dispose(state.Dependency);
sphereCollection.Dispose(state.Dependency);
boxCollection.Dispose(state.Dependency);
handles.Dispose(state.Dependency);
}
}
private static unsafe void ExtractCollider(Collider* colliderPointer,
NativeParallelMultiHashMap<byte, ColliderCollection> colliders,
Entity colliderEntity, float4x4 relative)
{
var type = colliderPointer->Type;
switch (type)
{
case ColliderType.Convex:
return;
case ColliderType.Triangle:
return;
case ColliderType.Quad:
return;
case ColliderType.Cylinder:
return;
case ColliderType.Mesh:
return;
case ColliderType.Compound:
var compoundCollider = (CompoundCollider*)colliderPointer;
for (int i = 0; i < compoundCollider->Children.Length; i++)
{
ref var child = ref compoundCollider->Children[i];
var colider = child.Collider;
ExtractCollider(colider, colliders, colliderEntity, new float4x4(child.CompoundFromChild));
}
return;
case ColliderType.Terrain:
return;
}
if (colliderPointer->GetCollisionResponse() == CollisionResponsePolicy.RaiseTriggerEvents)
return;
colliders.Add((byte)type, new ColliderCollection()
{
Collider = colliderPointer,
Entity = colliderEntity,
Transform = relative
});
}
}
[BurstCompile]
public partial struct InteractJob : IJobFor
{
[ReadOnly] public float PointSphereRadius;
[NativeDisableParallelForRestriction] public BufferLookup<Point> BufferLookup;
[ReadOnly] public Entity Entity;
[ReadOnly] public NativeArray<CapsuleInteraction>.ReadOnly CapsuleCollection;
[ReadOnly] public NativeArray<SphereInteraction>.ReadOnly SphereCollection;
[ReadOnly] public NativeArray<BoxInteraction>.ReadOnly BoxCollection;
[BurstCompile]
public void Execute(int index)
{
BufferLookup.TryGetBuffer(Entity, out var points);
var point = points[index];
if (point.State != PointState.Free)
return;
var shouldItUnlock = false;
for (int i = 0; i < CapsuleCollection.Length; i++)
{
var capsuleData = CapsuleCollection[i];
var positonA = capsuleData.PositionA;
var positonB = capsuleData.PositionB;
for (int j = 0; j < capsuleData.HowMany; j++)
{
var hasInteracted = InteractedCapsule(ref point, positonA, positonB, capsuleData.Radius);
if (!hasInteracted)
shouldItUnlock = true;
positonA += capsuleData.Velocity;
positonB += capsuleData.Velocity;
}
}
for (int i = 0; i < SphereCollection.Length; i++)
{
var sphereInteraction = SphereCollection[i];
var center = sphereInteraction.Center;
for (int j = 0; j < sphereInteraction.HowMany; j++)
{
var hasInteracted = InteractedSphere(ref point, center, sphereInteraction.Radius);
if (!hasInteracted)
shouldItUnlock = true;
center += sphereInteraction.Velocity;
}
}
for (int i = 0; i < BoxCollection.Length; i++)
{
var boxInteraction = BoxCollection[i];
var center = boxInteraction.Center;
for (int j = 0; j < boxInteraction.HowMany; j++)
{
var hasInteracted = InteractedBoxOBB(ref point, center, boxInteraction.HalfSize,
boxInteraction.Rotation);
if (!hasInteracted)
shouldItUnlock = true;
center += boxInteraction.Velocity;
}
}
if (shouldItUnlock)
point.IsLocked = false;
points[index] = point;
}
bool InteractedCapsule(ref Point point, float3 predicatedPosition, float3 highestPointInCapsule,
float capsuleRadius)
{
float3 closestPoint =
ClosestPointOnCapsuleAxis(predicatedPosition, highestPointInCapsule, point.Position);
// Calculate the distance between the closest point and the sphere's center
float distance = math.distance(closestPoint, point.Position);
var shouldInteract = distance <= PointSphereRadius + capsuleRadius;
if (shouldInteract)
{
SetNewPosition(ref point, closestPoint, distance, capsuleRadius);
return true;
}
return false;
}
private void SetNewPosition(ref Point point, float3 closestPoint, float distance, float capsuleRadius)
{
// Calculate the direction to move the sphere to resolve the intersection
float3 displacementDirection = math.normalizesafe(point.Position - closestPoint, new float3(1, 0, 0));
float displacementDistance = PointSphereRadius + capsuleRadius - distance;
var howMuchToMove = displacementDirection * displacementDistance;
//point.IsLocked = true;
point.PrevesPosition = point.Position;
point.Position += howMuchToMove;
}
private float3 ClosestPointOnCapsuleAxis(float3 a, float3 b, float3 p)
{
float3 ab = b - a;
float abLenSq = math.lengthsq(ab);
if (abLenSq < 1e-8f) return a;
float t = math.clamp(math.dot(p - a, ab) / abLenSq, 0f, 1f);
return a + t * ab;
}
bool InteractedSphere(ref Point point, float3 sphereCenter, float sphereRadius)
{
// Closest point on the sphere surface to the particle center
float3 closestPoint = sphereCenter;
// Distance from particle center to that closest point
float distance = math.distance(closestPoint, point.Position); // :contentReference[oaicite:1]{index=1}
//bool shouldInteract = distance <= PointSphereRadius + sphereRadius;
bool shouldInteract = distance <= PointSphereRadius + sphereRadius;
if (shouldInteract)
{
SetNewPosition(ref point, closestPoint, distance, sphereRadius);
return true;
}
return false;
}
bool InteractedBoxOBB(ref Point point, float3 boxCenter, float3 boxHalfExtents, quaternion boxRotation)
{
// Transform point into box local space (OBB -> AABB in local frame)
float3 pWorld = point.Position;
float3 pLocal = math.mul(math.inverse(boxRotation), (pWorld - boxCenter));
// Run the AABB logic in local space
float3 min = -boxHalfExtents;
float3 max = boxHalfExtents;
float3 closestLocal = math.clamp(pLocal, min, max);
float3 deltaLocal = pLocal - closestLocal;
float distSq = math.lengthsq(deltaLocal);
bool inside = distSq < 1e-12f;
if (!inside)
{
float dist = math.sqrt(distSq);
bool shouldInteract = dist <= PointSphereRadius;
if (!shouldInteract) return false;
float3 closestWorld = boxCenter + math.mul(boxRotation, closestLocal);
ResolveSphereVsBox(ref point, closestWorld, dist, inside: false);
return true;
}
else
{
// Inside: nearest face in local space
float3 absLocal = math.abs(pLocal);
float3 toFace = boxHalfExtents - absLocal;
int axis = 0;
float minToFace = toFace.x;
if (toFace.y < minToFace)
{
minToFace = toFace.y;
axis = 1;
}
if (toFace.z < minToFace)
{
minToFace = toFace.z;
axis = 2;
}
float3 faceLocal = pLocal;
float sign = (axis == 0 ? (pLocal.x >= 0f ? 1f : -1f)
: axis == 1 ? (pLocal.y >= 0f ? 1f : -1f)
: (pLocal.z >= 0f ? 1f : -1f));
if (axis == 0) faceLocal.x = sign * boxHalfExtents.x;
else if (axis == 1) faceLocal.y = sign * boxHalfExtents.y;
else faceLocal.z = sign * boxHalfExtents.z;
float distToFace = math.distance(pLocal, faceLocal);
float3 faceWorld = boxCenter + math.mul(boxRotation, faceLocal);
ResolveSphereVsBox(ref point, faceWorld, distToFace, inside: true);
return true;
}
}
private void ResolveSphereVsBox(ref Point point, float3 surfacePoint, float distanceToSurface, bool inside)
{
// If outside: move away from surface by (r - distance)
// If inside : move outward past surface by (r + distance)
float3 p = point.Position;
float3 dir;
float moveDist;
if (!inside)
{
dir = math.normalizesafe(p - surfacePoint, new float3(1, 0, 0));
moveDist = PointSphereRadius - distanceToSurface;
}
else
{
// outward direction is from point -> surface (then continue outward)
dir = math.normalizesafe(surfacePoint - p, new float3(1, 0, 0));
moveDist = PointSphereRadius + distanceToSurface;
}
float3 move = dir * moveDist;
point.PrevesPosition = point.Position;
point.Position += move;
}
}
}