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 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);
}
}
}
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 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;
}
public static ExchangeRecipeBufferElement GetRecipe(this DynamicBuffer <ExchangeRecipeBufferElement> buffer,
FixedList128 <FixedString32> sourceResources, FixedList128 <FixedString32> resultResources)
{
ExchangeRecipeBufferElement output = default;
for (int i = 0; i < buffer.Length; i++)
{
var recipe = buffer[i];
if (sourceResources.Length != recipe.SourceResourcesNames.Length ||
resultResources.Length != recipe.ResultResourcesNames.Length)
{
continue;
}
var flag = true;
for (int j = 0; j < sourceResources.Length && flag; j++)
{
flag &= recipe.SourceResourcesNames.Contains(sourceResources[j]);
}
for (int j = 0; j < resultResources.Length && flag; j++)
{
flag &= recipe.ResultResourcesNames.Contains(resultResources[j]);
}
if (flag)
{
output = recipe;
break;
}
}
return(output);
}
public static bool BoxIntersectsBox(FixedList512 <float3> axes, FixedList128 <float3> verticesA, FixedList128 <float3> verticesB, out float minOverlap, out float3 mtvAxis)
{
minOverlap = float.MaxValue;
mtvAxis = float3.zero;
for (int i = 0; i < axes.Length; i++)
{
float3 axis = axes[i];
if (axis.x == 0 && axis.y == 0 && axis.z == 0)
{
continue;
}
float2 minMaxA = GetMinMaxProjectionSAT(verticesA, axis);
float2 minMaxB = GetMinMaxProjectionSAT(verticesB, axis);
float overlap = GetLineOverLap(minMaxA.x, minMaxB.x, minMaxA.y, minMaxB.y);
if (overlap <= 0)
{
return(false);
}
else if (overlap < minOverlap)
{
minOverlap = overlap;
mtvAxis = axis;
}
}
return(true);
}
public static float3 ClosestPointBoxPoint(float3 point, float3 obbCenter, FixedList128 <float3> boxNormals, FixedList128 <float> boxExtents)
{
// c = center point, u = directional unit vector
// all points contained by obb b can be written as
// s = c + au0 + bu1 + cu2
// point p in orld space can be represented as point q in obb space as
// q = c + xu0 + yu1 + zu2
// x = (p-c) dot u0 , y = (p-c) dot u1 , z = (p-c) dot u2
float3 d = point - obbCenter;
float3 q = obbCenter;
for (int i = 0; i < 3; i++)
{
float dist = math.dot(d, boxNormals[i]);
dist = math.max(dist, -boxExtents[i]);
dist = math.min(dist, boxExtents[i]);
q += dist * boxNormals[i];
}
return(q);
}
public static FixedList512 <float3> GetProjectionAxesOBBSAT(FixedList128 <float3> verticesA, FixedList128 <float3> verticesB)
{
var projectionAxes = new FixedList512 <float3>();
FixedList128 <float3> normalAxesA = GetAxisNormalsOBB(verticesA[0], verticesA[1], verticesA[3], verticesA[4]);
FixedList128 <float3> normalAxesB = GetAxisNormalsOBB(verticesB[0], verticesB[1], verticesB[3], verticesB[4]);
for (int i = 0; i < normalAxesA.Length; i++)
{
projectionAxes.Add(normalAxesA[i]);
}
for (int i = 0; i < normalAxesB.Length; i++)
{
projectionAxes.Add(normalAxesB[i]);
}
projectionAxes.Add(math.cross(normalAxesA[0], normalAxesB[0]));
projectionAxes.Add(math.cross(normalAxesA[0], normalAxesB[1]));
projectionAxes.Add(math.cross(normalAxesA[0], normalAxesB[2]));
projectionAxes.Add(math.cross(normalAxesA[1], normalAxesB[0]));
projectionAxes.Add(math.cross(normalAxesA[1], normalAxesB[1]));
projectionAxes.Add(math.cross(normalAxesA[1], normalAxesB[2]));
projectionAxes.Add(math.cross(normalAxesA[2], normalAxesB[0]));
projectionAxes.Add(math.cross(normalAxesA[2], normalAxesB[1]));
projectionAxes.Add(math.cross(normalAxesA[2], normalAxesB[2]));
return(projectionAxes);
}
/// <summary>
/// Modifies this container to remove all values that are present in the other container.
/// </summary>
/// <typeparam name="T">Source type of elements</typeparam>
/// <param name="container">Container to modify.</param>
/// <param name="other">The container to compare to this container.</param>
public static void ExceptWith <T>(this NativeHashSet <T> container, FixedList128 <T> other)
where T : unmanaged, IEquatable <T>
{
foreach (var item in other)
{
container.Remove(item);
}
}
/// <summary>
/// Modifies this container to contain values from both containers.
/// </summary>
/// <typeparam name="T">Source type of elements</typeparam>
/// <param name="container">Container to modify.</param>
/// <param name="other">The container to compare to this container.</param>
public static void UnionWith <T>(this UnsafeHashSet <T> container, FixedList128 <T> other)
where T : unmanaged, IEquatable <T>
{
foreach (var item in other)
{
container.Add(item);
}
}
public static FixedList128 <float3> GetRotatedVerticesOBB(FixedList128 <float3> vertices, float3 origin, float3 rotation)
{
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = RotatePoint3D(vertices[i], origin, rotation.y, rotation.z, rotation.x);
}
return(vertices);
}
public Enumerator(int2 c, int radius)
{
points = new FixedList128 <int2>();
p.x = 0;
p.y = radius;
this.c = c;
d = 3 - 2 * radius;
i = -1;
SubsequencePoints(ref points, c, p);
}
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); });
}
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 bool SphereIntersectsTerrainTriangle(float3 spherePos, FixedList128 <float3> triVertices, out float3 closestPoint)
{
if (PointInsideTriangle2D(triVertices[0].x, triVertices[0].z, triVertices[1].x, triVertices[1].z, triVertices[2].x, triVertices[2].z, spherePos.x, spherePos.y))
{
closestPoint = ClosestPointTrianglePointNew(spherePos, triVertices[0], triVertices[1], triVertices[2]);
return(true);
}
else
{
closestPoint = float3.zero;
return(false);
}
}
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 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 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 static float2 GetMinMaxProjectionSAT(FixedList128 <float3> vertices, float3 axis)
{
float projMin = float.MaxValue;
float projMax = float.MinValue;
for (int i = 0; i < vertices.Length; i++)
{
float p = math.dot(vertices[i], axis);
projMin = math.min(p, projMin);
projMax = math.max(p, projMax);
}
return(new float2(projMin, projMax));
}
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);
}
/// <summary>
/// Modifies this container to keep only values that are present in both containers.
/// </summary>
/// <typeparam name="T">Source type of elements</typeparam>
/// <param name="container">Container to modify.</param>
/// <param name="other">The container to compare to this container.</param>
public static void IntersectWith <T>(this UnsafeHashSet <T> container, FixedList128 <T> other)
where T : unmanaged, IEquatable <T>
{
var result = new UnsafeList <T>(container.Count(), Allocator.Temp);
foreach (var item in other)
{
if (container.Contains(item))
{
result.Add(item);
}
}
container.Clear();
container.UnionWith(result);
result.Dispose();
}
private void CollisionBoxBox()
{
FixedList128 <float3> verticesA = ColPhysics.GetAABBVerticesOBB(posA, extentsA);
FixedList128 <float3> verticesB = ColPhysics.GetAABBVerticesOBB(posB, extentsB);
verticesA = ColPhysics.GetRotatedVerticesOBB(verticesA, posA, rotA);
verticesB = ColPhysics.GetRotatedVerticesOBB(verticesB, posB, rotB);
FixedList512 <float3> projectionAxes = ColPhysics.GetProjectionAxesOBBSAT(verticesA, verticesB);
if (ColPhysics.BoxIntersectsBox(projectionAxes, verticesA, verticesB, out float minOverlap, out float3 mtvAxis))
{
isColliding = true;
if (resolveCollisions)
{
ColPhysics.ResolveBoxCollision(ref posA, ref posB, minOverlap, mtvAxis);
}
}
}
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));
}
}
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 static bool SphereIntersectsOrientedTriangle(float3 spherePos, float sphereRadius, FixedList128 <float3> triVertices)
{
float3 closestPoint = ClosestPointTrianglePointNew(spherePos, triVertices[0], triVertices[1], triVertices[2]);
return(math.distance(closestPoint, spherePos) < sphereRadius);
}
public static bool CapsuleIntersectsBox(float3x2 capsuleSpheres, float3 obbPos, float capsuleRadius, FixedList128 <float3> boxNormals, FixedList128 <float> boxExtents)
{
float3 closestPoint = ClosestPointLineSegementPoint(capsuleSpheres.c1, capsuleSpheres.c0, obbPos);
return(SphereIntersectsBox(closestPoint, capsuleRadius, obbPos, boxNormals, boxExtents));
}
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);
}
public static bool SphereIntersectsBox(float3 spherePos, float sphereRadius, float3 boxPos, FixedList128 <float3> boxNormals, FixedList128 <float> boxExtents)
{
float3 closestPoint = ClosestPointBoxPoint(spherePos, boxPos, boxNormals, boxExtents);
return(math.distancesq(closestPoint, spherePos) < sphereRadius);
}
