void IJobParallelFor.Execute(int index)
{
var pRW = pRWPoints + index;
if (IsEnableFloor)
{
if (pRW->Position.y <= FloorHeight)
{
pRW->Position.y = FloorHeight;
}
}
if (IsEnableCollider)
{
for (int i = 0; i < ColliderCount; ++i)
{
Collider * pCollider = pColliders + i;
ColliderEx *pColliderEx = pColliderExs + i;
if (pCollider->Height <= 0.0f)
{
PushoutFromSphere(pCollider, pColliderEx, ref pRW->Position);
}
else
{
PushoutFromCapsule(pCollider, pColliderEx, ref pRW->Position);
}
}
}
}
void PushoutFromCapsule(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
{
var capsuleVec = pColliderEx->Direction;
var capsuleVecNormal = capsuleVec.normalized;
var capsulePos = pColliderEx->Position;
var targetVec = point - capsulePos;
var distanceOnVec = Vector3.Dot(capsuleVecNormal, targetVec);
if (distanceOnVec <= EPSILON)
{
PushoutFromSphere(capsulePos, pCollider->RadiusHead, ref point);
return;
}
else if (distanceOnVec >= pCollider->Height)
{
PushoutFromSphere(capsulePos + capsuleVec, pCollider->RadiusTail, ref point);
return;
}
else
{
var positionOnVec = capsulePos + (capsuleVecNormal * distanceOnVec);
var pushoutVec = point - positionOnVec;
var sqrPushoutDistance = pushoutVec.sqrMagnitude;
if (sqrPushoutDistance > EPSILON)
{
var Radius = pCollider->RadiusHead + (pCollider->RadiusTail - pCollider->RadiusHead) * distanceOnVec;
if (sqrPushoutDistance < Radius * Radius)
{
var pushoutDistance = Mathf.Sqrt(sqrPushoutDistance);
point = positionOnVec + pushoutVec * Radius / pushoutDistance;
return;
}
}
}
}
static void PushoutFromCapsule(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
{
var capsuleVec = pColliderEx->Direction;
var capsulePos = pColliderEx->Position;
var targetVec = point - capsulePos;
var radius = pCollider->Radius;
var distanceOnVec = Vector3.Dot(capsuleVec, targetVec);
if (distanceOnVec <= 0.0f)
{
PushoutFromSphere(capsulePos, radius, ref point);
}
else if (distanceOnVec >= pCollider->Height)
{
PushoutFromSphere(capsulePos + capsuleVec * distanceOnVec, radius, ref point);
}
else
{
var positionOnVec = capsulePos + (capsuleVec * distanceOnVec);
var pushoutVec = point - positionOnVec;
var distanceSquared = pushoutVec.sqrMagnitude;
if (distanceSquared > Epsilon && distanceSquared < radius * radius)
{
var distance = Mathf.Sqrt(distanceSquared);
point = positionOnVec + pushoutVec * radius / distance;
}
}
}
bool CollisionDetection(Collider *pCollider, ColliderEx *pColliderEx, Vector3 point1, Vector3 point2, out Vector3 pointOnLine, out Vector3 pointOnCollider, out float Radius)
{
if (pCollider->Height <= EPSILON)
{
var direction = point2 - point1;
var directionLength = direction.magnitude;
direction /= directionLength;
var toCenter = pColliderEx->Position - point1;
var dot = Vector3.Dot(direction, toCenter);
var pointOnDirection = direction * Mathf.Clamp(dot, 0.0f, directionLength);
pointOnCollider = pColliderEx->Position;
pointOnLine = pointOnDirection + point1;
Radius = pCollider->RadiusHead;
if ((pointOnCollider - pointOnLine).sqrMagnitude > pCollider->RadiusHead * pCollider->RadiusHead)
{
return(false);
}
return(true);
}
else
{
var capsuleDir = pColliderEx->Direction;
var capsulePos = pColliderEx->Position;
var pointDir = point2 - point1;
float t1, t2;
var sqrDistance = ComputeNearestPoints(capsulePos, capsuleDir, point1, pointDir, out t1, out t2, out pointOnCollider, out pointOnLine);
t1 = Mathf.Clamp01(t1);
Radius = pCollider->RadiusHead + (pCollider->RadiusTail - pCollider->RadiusHead) * t1;
if (sqrDistance > Radius * Radius)
{
pointOnCollider = Vector3.zero;
pointOnLine = Vector3.zero;
return(false);
}
t2 = Mathf.Clamp01(t2);
pointOnCollider = capsulePos + capsuleDir * t1;
pointOnLine = point1 + pointDir * t2;
return((pointOnCollider - pointOnLine).sqrMagnitude <= Radius * Radius);
}
}
static bool HitTest(Collider *pCollider, ColliderEx *pColliderEx, Vector3 point1, Vector3 point2, out Vector3 pointOnLine, out Vector3 pointOnCollider)
{
if (pCollider->IsSphere)
{
var direction = point2 - point1;
var directionLength = direction.magnitude;
direction /= directionLength;
var toCenter = pColliderEx->Position - point1;
var dot = Vector3.Dot(direction, toCenter);
var pointOnDirection = direction * Mathf.Clamp(dot, 0.0f, directionLength);
pointOnCollider = pColliderEx->Position;
pointOnLine = pointOnDirection + point1;
return(!((pointOnCollider - pointOnLine).sqrMagnitude > pCollider->Radius * pCollider->Radius));
}
else
{
var capsuleDir = pColliderEx->Direction;
var capsulePos = pColliderEx->Position;
var pointDir = point2 - point1;
var sqrDistance = ComputeNearestPoints(capsulePos, capsuleDir, point1, pointDir, out float t1, out float t2, out pointOnCollider, out pointOnLine);
if (sqrDistance > pCollider->Radius * pCollider->Radius)
{
pointOnCollider = Vector3.zero;
pointOnLine = Vector3.zero;
return(false);
}
t1 = Mathf.Clamp01(t1);
t2 = Mathf.Clamp01(t2);
pointOnCollider = capsulePos + capsuleDir * t1;
pointOnLine = point1 + pointDir * t2;
return((pointOnCollider - pointOnLine).sqrMagnitude <= pCollider->Radius * pCollider->Radius);
}
}
void PushoutFromSphere(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
{
PushoutFromSphere(pColliderEx->Position, pCollider->RadiusHead, ref point);
}
void IJobParallelFor.Execute(int index)
{
var constraint = pConstraints + index;
var RptA = pRPoints + constraint->IndexA;
var RptB = pRPoints + constraint->IndexB;
float WeightA = RptA->Weight;
float WeightB = RptB->Weight;
if ((WeightA <= EPSILON) && (WeightB <= EPSILON))
{
return;
}
var RWptA = pRWPoints + constraint->IndexA;
var RWptB = pRWPoints + constraint->IndexB;
var Direction = RWptB->Position - RWptA->Position;
float Distance = Direction.magnitude;
float Force = (Distance - constraint->Length) * SpringK;
bool IsShrink = Force >= 0.0f;
float ConstraintPower;
switch (constraint->Type)
{
case SPCRJointDynamicsController.ConstraintType.Structural_Vertical:
ConstraintPower = IsShrink
? constraint->Shrink * (RptA->StructuralShrinkVertical + RptB->StructuralShrinkVertical)
: constraint->Stretch * (RptA->StructuralStretchVertical + RptB->StructuralStretchVertical);
break;
case SPCRJointDynamicsController.ConstraintType.Structural_Horizontal:
ConstraintPower = IsShrink
? constraint->Shrink * (RptA->StructuralShrinkHorizontal + RptB->StructuralShrinkHorizontal)
: constraint->Stretch * (RptA->StructuralStretchHorizontal + RptB->StructuralStretchHorizontal);
break;
case SPCRJointDynamicsController.ConstraintType.Shear:
ConstraintPower = IsShrink
? constraint->Shrink * (RptA->ShearShrink + RptB->ShearShrink)
: constraint->Stretch * (RptA->ShearStretch + RptB->ShearStretch);
break;
case SPCRJointDynamicsController.ConstraintType.Bending_Vertical:
ConstraintPower = IsShrink
? constraint->Shrink * (RptA->BendingShrinkVertical + RptB->BendingShrinkVertical)
: constraint->Stretch * (RptA->BendingStretchVertical + RptB->BendingStretchVertical);
break;
case SPCRJointDynamicsController.ConstraintType.Bending_Horizontal:
ConstraintPower = IsShrink
? constraint->Shrink * (RptA->BendingShrinkHorizontal + RptB->BendingShrinkHorizontal)
: constraint->Stretch * (RptA->BendingStretchHorizontal + RptB->BendingStretchHorizontal);
break;
default:
ConstraintPower = 0.0f;
break;
}
if (ConstraintPower > 0.0f)
{
var Displacement = Direction.normalized * (Force * ConstraintPower);
float WightAB = WeightA + WeightB;
RWptA->Position += Displacement * WeightA / WightAB;
RWptB->Position -= Displacement * WeightB / WightAB;
}
if (constraint->IsCollision == 0)
{
return;
}
float Friction = 0.0f;
for (int i = 0; i < ColliderCount; ++i)
{
Collider * pCollider = pColliders + i;
ColliderEx *pColliderEx = pColliderExs + i;
float Radius;
Vector3 pointOnLine, pointOnCollider;
if (CollisionDetection(pCollider, pColliderEx, RWptA->Position, RWptB->Position, out pointOnLine, out pointOnCollider, out Radius))
{
var Pushout = pointOnLine - pointOnCollider;
var PushoutDistance = Pushout.magnitude;
var pointDistance = (RWptB->Position - RWptA->Position).magnitude * 0.5f;
var rateP1 = Mathf.Clamp01((pointOnLine - RWptA->Position).magnitude / pointDistance);
var rateP2 = Mathf.Clamp01((pointOnLine - RWptB->Position).magnitude / pointDistance);
Pushout /= PushoutDistance;
Pushout *= Mathf.Max(Radius - PushoutDistance, 0.0f);
RWptA->Position += Pushout * rateP2;
RWptB->Position += Pushout * rateP1;
var Dot = Vector3.Dot(Vector3.up, (pointOnLine - pointOnCollider).normalized);
Friction = Mathf.Max(Friction, pCollider->Friction * Mathf.Clamp01(Dot));
}
}
RWptA->Friction = Mathf.Max(Friction, RWptA->Friction);
RWptB->Friction = Mathf.Max(Friction, RWptB->Friction);
}
