private void CollisionCapsuleBox()
{
FixedList128 <float3> verticesOBB = ColPhysics.GetAABBVerticesOBB(posB, extentsB);
verticesOBB = ColPhysics.GetRotatedVerticesOBB(verticesOBB, posB, rotB);
FixedList128 <float3> normalAxesOBB = ColPhysics.GetAxisNormalsOBB(verticesOBB[0], verticesOBB[1], verticesOBB[3], verticesOBB[4]);
FixedList128 <float> exte = new FixedList128 <float>();
exte.Add(extentsB.x);
exte.Add(extentsB.y);
exte.Add(extentsB.z);
float3x2 capsuleTips = ColPhysics.GetCapsuleEndPoints(posA, rotA, extentsA.y * 2);
float3x2 capsuleSpheres = ColPhysics.GetCapsuleEndSpheres(capsuleTips.c0, capsuleTips.c1, extentsA.x);
if (ColPhysics.CapsuleIntersectsBox(capsuleSpheres, posB, extentsA.x, normalAxesOBB, exte, out float distance))
{
isColliding = true;
if (resolveCollisions)
{
ColPhysics.ResolveSphereBoxCollision(ref posA, extentsA.x, ref posB, distance);
}
}
}
PhysicsJoint CreateRigidbodyConstraints(RigidTransform offset)
{
var joint = new PhysicsJoint {
BodyAFromJoint = BodyFrame.Identity,
BodyBFromJoint = offset
};
var constraints = new FixedList128 <Constraint>();
constraints.Add(new Constraint {
ConstrainedAxes = new bool3(false, false, true),
Type = ConstraintType.Linear,
Min = 0,
Max = 0,
SpringFrequency = Constraint.DefaultSpringFrequency,
SpringDamping = Constraint.DefaultSpringDamping
});
constraints.Add(new Constraint {
ConstrainedAxes = new bool3(true, true, true),
Type = ConstraintType.Angular,
Min = 0,
Max = 0,
SpringFrequency = Constraint.DefaultSpringFrequency,
SpringDamping = Constraint.DefaultSpringDamping
});
joint.SetConstraints(constraints);
return(joint);
}
private void CollisionSphereBox()
{
float3 spherePos = posA;
float3 obbPos = posB;
float3 obbRot = rotB;
float3 obbHalfSize = extentsB;
float sphereRadius = extentsA.x;
FixedList128 <float3> vertices = ColPhysics.GetAABBVerticesOBB(obbPos, obbHalfSize); // calculated once at startup forever stored with entity
vertices = ColPhysics.GetRotatedVerticesOBB(vertices, obbPos, obbRot);
FixedList128 <float> extents = new FixedList128 <float>(); // calculated once at startup forever stored with entity
extents.Add(extentsB.x);
extents.Add(extentsB.y);
extents.Add(extentsB.z);
FixedList128 <float3> axisNormals = ColPhysics.GetAxisNormalsOBB(vertices[0], vertices[1], vertices[3], vertices[4]);
if (ColPhysics.SphereIntersectsBox(spherePos, sphereRadius, obbPos, axisNormals, extents, out float distance))
{
isColliding = true;
if (resolveCollisions)
{
ColPhysics.ResolveSphereBoxCollision(ref spherePos, sphereRadius, ref obbPos, distance);
posA = spherePos;
posB = obbPos;
}
}
}
// SPHERE TRIANGLE
public void HandleSphereTriangle()
{
float3 spherePos = posA;
float sphereRadius = extentsA.x;
float3 triPos = posB;
float3 triSize = extentsB;
float3 triRot = rotB;
float3[] triangleVerts = GetVerticesOfTriangle(triPos, triSize, triRot); // would already be stored
FixedList128 <float3> triVertices = new FixedList128 <float3>();
triVertices.Add(triangleVerts[0]);
triVertices.Add(triangleVerts[1]);
triVertices.Add(triangleVerts[2]);
if (ColPhysics.SphereIntersectsOrientedTriangle(spherePos, sphereRadius, triVertices, out float3 closestPoint, out float distance))
{
isColliding = true;
if (drawMarkers)
{
display.DrawMarker(closestPoint);
}
if (resolveCollisions)
{
ColPhysics.ResolveSphereTriangleCollision(ref spherePos, sphereRadius, distance, closestPoint);
}
posA = spherePos;
}
display.DrawSphereTriangle(posA, posB, rotB, extentsA.x, extentsB);
}
public static FixedList128 <float3> GetAxisNormalsOBB(float3 bl, float3 br, float3 tl, float3 bl2)
{
FixedList128 <float3> axes = new FixedList128 <float3>();
axes.Add(math.normalize(br - bl));
axes.Add(math.normalize(tl - bl));
axes.Add(math.normalize(bl2 - bl));
return(axes);
}
public void FixedList128intGenericHasExpectedCapacity()
{
var list = new FixedList128 <int>();
var expectedCapacity = list.Capacity;
for (int i = 0; i < expectedCapacity; ++i)
{
list.Add((int)i);
}
Assert.Throws <IndexOutOfRangeException> (() => { list.Add((int)expectedCapacity); });
}
public void FixedList128intGenericRemoveRange()
{
var list = new FixedList128 <int>();
list.Add(0);
list.Add(3);
list.Add(3);
list.Add(1);
list.Add(2);
list.RemoveRange(1, 2);
for (var i = 0; i < 3; ++i)
{
Assert.AreEqual(i, list[i]);
}
}
public void FixedList128intGenericInsertRange()
{
var list = new FixedList128 <int>();
list.Add(0);
list.Add(3);
list.Add(4);
list.InsertRange(1, 2);
list[1] = 1;
list[2] = 2;
for (var i = 0; i < 5; ++i)
{
Assert.AreEqual(i, list[i]);
}
}
public ExchangeRecipeBufferElement(ExchangeItem[] sourceItems, ExchangeItem[] resultItems, float timeRequired = 0, bool isCrafting = false)
{
FixedList128 <FixedString32> sourceResourcesNames = new FixedList128 <FixedString32>();
FixedListFloat32 sourceResourcesMultipliers = new FixedListFloat32();
FixedList128 <FixedString32> resultResourcesNames = new FixedList128 <FixedString32>();
FixedListFloat32 resultResourcesMultipliers = new FixedListFloat32();
for (int i = 0; i < sourceItems.Length; i++)
{
sourceResourcesNames.Add(sourceItems[i].ResourceName);
sourceResourcesMultipliers.Add(sourceItems[i].Multiplier);
}
for (int i = 0; i < resultItems.Length; i++)
{
resultResourcesNames.Add(resultItems[i].ResourceName);
resultResourcesMultipliers.Add(resultItems[i].Multiplier);
}
SourceResourcesNames = sourceResourcesNames;
SourceResourcesMultipliers = sourceResourcesMultipliers;
ResultResourcesNames = resultResourcesNames;
ResultResourcesMultipliers = resultResourcesMultipliers;
Hash = SourceResourcesNames.GetHashCode()
+ SourceResourcesMultipliers.GetHashCode()
+ ResultResourcesNames.GetHashCode()
+ ResultResourcesMultipliers.GetHashCode();
IsCrafting = isCrafting;
}
static void SubsequencePoints(ref FixedList128 <int2> output,
int2 c,
int2 p)
{
output.Add(new int2(c.x + p.x, c.y + p.y));
output.Add(new int2(c.x - p.x, c.y + p.y));
output.Add(new int2(c.x + p.x, c.y - p.y));
output.Add(new int2(c.x - p.x, c.y - p.y));
output.Add(new int2(c.x + p.y, c.y + p.x));
output.Add(new int2(c.x - p.y, c.y + p.x));
output.Add(new int2(c.x + p.y, c.y - p.x));
output.Add(new int2(c.x - p.y, c.y - p.x));
}
public static PhysicsJoint CreateLimitDOFJoint(RigidTransform offset, bool3 linearLocks, bool3 angularLocks)
{
var constraints = new FixedList128 <Constraint>();
if (math.any(linearLocks))
{
constraints.Add(new Constraint
{
ConstrainedAxes = linearLocks,
Type = ConstraintType.Linear,
Min = 0,
Max = 0,
SpringFrequency = Constraint.DefaultSpringFrequency,
SpringDamping = Constraint.DefaultSpringDamping
});
}
if (math.any(angularLocks))
{
constraints.Add(new Constraint
{
ConstrainedAxes = angularLocks,
Type = ConstraintType.Angular,
Min = 0,
Max = 0,
SpringFrequency = Constraint.DefaultSpringFrequency,
SpringDamping = Constraint.DefaultSpringDamping
});
}
var joint = new PhysicsJoint
{
BodyAFromJoint = BodyFrame.Identity,
BodyBFromJoint = offset
};
joint.SetConstraints(constraints);
return(joint);
}
public void FixedList128intGenericSort()
{
var list = new FixedList128 <int>();
for (var i = 0; i < 5; ++i)
{
list.Add((int)(4 - i));
}
list.Sort();
for (var i = 0; i < 5; ++i)
{
Assert.AreEqual(i, list[i]);
}
}
public static FixedList128 <float3> GetAABBVerticesOBB(float3 origin, float3 halfSize)
{
// -z, bl,br,tr,tl +z, bl,br,tr,tl
FixedList128 <float3> verts = new FixedList128 <float3>();
verts.Add(origin + new float3(-halfSize.x, -halfSize.y, -halfSize.z));
verts.Add(origin + new float3(+halfSize.x, -halfSize.y, -halfSize.z));
verts.Add(origin + new float3(+halfSize.x, +halfSize.y, -halfSize.z));
verts.Add(origin + new float3(-halfSize.x, +halfSize.y, -halfSize.z));
verts.Add(origin + new float3(-halfSize.x, -halfSize.y, +halfSize.z));
verts.Add(origin + new float3(+halfSize.x, -halfSize.y, +halfSize.z));
verts.Add(origin + new float3(+halfSize.x, +halfSize.y, +halfSize.z));
verts.Add(origin + new float3(-halfSize.x, +halfSize.y, +halfSize.z));
return(verts);
}
public void FixedList128intGenericHasExpectedLayout()
{
var actual = new FixedList128 <int>();
for (var i = 0; i < 31; ++i)
{
actual.Add((int)i);
}
unsafe
{
var e = stackalloc byte[128];
e[0] = (byte)((31 >> 0) & 0xFF);
e[1] = (byte)((31 >> 8) & 0xFF);
for (var i = 0; i < 31; ++i)
{
var s = (int)i;
UnsafeUtility.MemCpy(e + 2 + FixedList.PaddingBytes <int>() + sizeof(int) * i, &s, sizeof(int));
}
Assert.AreEqual(0, UnsafeUtility.MemCmp(e, &actual.length, 128));
}
}
PhysicsJoint CreateConfigurableJoint(
quaternion jointFrameOrientation,
LegacyJoint joint, bool3 linearLocks, bool3 linearLimited, SoftJointLimit linearLimit, SoftJointLimitSpring linearSpring, bool3 angularFree, bool3 angularLocks,
bool3 angularLimited, SoftJointLimit lowAngularXLimit, SoftJointLimit highAngularXLimit, SoftJointLimitSpring angularXLimitSpring, SoftJointLimit angularYLimit,
SoftJointLimit angularZLimit, SoftJointLimitSpring angularYZLimitSpring)
{
var constraints = new FixedList128 <Constraint>();
// TODO: investigate mapping PhysX spring and damping to Unity Physics SpringFrequency and SpringDamping
var springFrequency = Constraint.DefaultSpringFrequency;
var springDamping = Constraint.DefaultSpringDamping;
if (angularLimited[0])
{
constraints.Add(Constraint.Twist(0, math.radians(new FloatRange(-highAngularXLimit.limit, -lowAngularXLimit.limit).Sorted()), springFrequency, springDamping));
}
if (angularLimited[1])
{
constraints.Add(Constraint.Twist(1, math.radians(new FloatRange(-angularYLimit.limit, angularYLimit.limit).Sorted()), springFrequency, springDamping));
}
if (angularLimited[2])
{
constraints.Add(Constraint.Twist(2, math.radians(new FloatRange(-angularZLimit.limit, angularZLimit.limit).Sorted()), springFrequency, springDamping));
}
if (math.any(linearLimited))
{
var distanceRange = new FloatRange(-linearLimit.limit, linearLimit.limit).Sorted();
constraints.Add(new Constraint
{
ConstrainedAxes = linearLimited,
Type = ConstraintType.Linear,
Min = math.csum((int3)linearLimited) == 1 ? distanceRange.Min : 0f,
Max = distanceRange.Max,
SpringFrequency = springFrequency,
SpringDamping = springDamping
});
}
if (math.any(linearLocks))
{
constraints.Add(new Constraint
{
ConstrainedAxes = linearLocks,
Type = ConstraintType.Linear,
Min = 0,
Max = 0,
SpringFrequency = springFrequency,
SpringDamping = springDamping
});
}
if (math.any(angularLocks))
{
constraints.Add(new Constraint
{
ConstrainedAxes = angularLocks,
Type = ConstraintType.Angular,
Min = 0,
Max = 0,
SpringFrequency = springFrequency,
SpringDamping = springDamping
});
}
RigidTransform worldFromBodyA = Math.DecomposeRigidBodyTransform(joint.transform.localToWorldMatrix);
RigidTransform worldFromBodyB = joint.connectedBody == null
? RigidTransform.identity
: Math.DecomposeRigidBodyTransform(joint.connectedBody.transform.localToWorldMatrix);
var legacyWorldFromJointA = math.mul(
new RigidTransform(joint.transform.rotation, joint.transform.position),
new RigidTransform(jointFrameOrientation, joint.anchor)
);
var bodyAFromJoint = new BodyFrame(math.mul(math.inverse(worldFromBodyA), legacyWorldFromJointA));
var connectedEntity = GetPrimaryEntity(joint.connectedBody);
var isConnectedBodyConverted =
joint.connectedBody == null || connectedEntity != Entity.Null;
RigidTransform bFromA = isConnectedBodyConverted ? math.mul(math.inverse(worldFromBodyB), worldFromBodyA) : worldFromBodyA;
RigidTransform bFromBSource =
isConnectedBodyConverted ? RigidTransform.identity : worldFromBodyB;
var bodyBFromJoint = new BodyFrame
{
Axis = math.mul(bFromA.rot, bodyAFromJoint.Axis),
PerpendicularAxis = math.mul(bFromA.rot, bodyAFromJoint.PerpendicularAxis),
Position = math.mul(bFromBSource, new float4(joint.connectedAnchor, 1f)).xyz
};
var jointData = new PhysicsJoint
{
BodyAFromJoint = bodyAFromJoint,
BodyBFromJoint = bodyBFromJoint
};
jointData.SetConstraints(constraints);
return(jointData);
}
