// followed by variable sized convex hull data
#region Construction
// Create a convex collider from the given point cloud.
public static unsafe BlobAssetReference <Collider> Create(
NativeArray <float3> points, float convexRadius,
float3?scale = null, CollisionFilter?filter = null, Material?material = null)
{
if (convexRadius < 0.0f || !math.isfinite(convexRadius))
{
throw new ArgumentException("Tried to create ConvexCollider with invalid convex radius");
}
// Build convex hull
int verticesCapacity = points.Length;
int triangleCapacity = 2 * verticesCapacity;
var vertices = (ConvexHullBuilder.Vertex *)UnsafeUtility.Malloc(verticesCapacity * sizeof(ConvexHullBuilder.Vertex), 16, Allocator.Temp);
var triangles = (ConvexHullBuilder.Triangle *)UnsafeUtility.Malloc(triangleCapacity * sizeof(ConvexHullBuilder.Triangle), 16, Allocator.Temp);
var builder = new ConvexHullBuilder(vertices, verticesCapacity, triangles, triangleCapacity);
float3 s = scale ?? new float3(1);
foreach (float3 point in points)
{
if (math.any(!math.isfinite(point)))
{
throw new ArgumentException("Tried to create ConvexCollider with invalid points");
}
builder.AddPoint(point * s);
}
// TODO: shrink by convex radius
// Build face information
float maxAngle = 0.1f * (float)math.PI / 180.0f;
builder.BuildFaceIndices(maxAngle);
// Simplify the hull until it fits requirements
// TODO.ma this is just a failsafe. We need to think about user-controlled simplification settings & how to warn the user if their shape is too complex.
{
const int maxVertices = 252; // as per Havok
float maxSimplificationError = 1e-3f;
int iterations = 0;
while (builder.Vertices.PeakCount > maxVertices)
{
if (iterations++ > 10) // don't loop forever
{
Assert.IsTrue(false);
return(new BlobAssetReference <Collider>());
}
builder.SimplifyVertices(maxSimplificationError);
builder.BuildFaceIndices();
maxSimplificationError *= 2.0f;
}
}
// Convert hull to compact format
var tempHull = new TempHull(ref builder);
// Allocate collider
int totalSize = UnsafeUtility.SizeOf <ConvexCollider>();
totalSize += tempHull.Vertices.Count * sizeof(float3);
totalSize = Math.NextMultipleOf16(totalSize); // planes currently must be aligned for Havok
totalSize += tempHull.Planes.Count * sizeof(Plane);
totalSize += tempHull.Faces.Count * sizeof(ConvexHull.Face);
totalSize += tempHull.FaceVertexIndices.Count * sizeof(short);
totalSize += tempHull.VertexEdges.Count * sizeof(ConvexHull.Edge);
totalSize += tempHull.FaceLinks.Count * sizeof(ConvexHull.Edge);
ConvexCollider *collider = (ConvexCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
// Initialize it
{
UnsafeUtility.MemClear(collider, totalSize);
collider->MemorySize = totalSize;
collider->m_Header.Type = ColliderType.Convex;
collider->m_Header.CollisionType = CollisionType.Convex;
collider->m_Header.Version = 0;
collider->m_Header.Magic = 0xff;
collider->m_Header.Filter = filter ?? CollisionFilter.Default;
collider->m_Header.Material = material ?? Material.Default;
ref var hull = ref collider->ConvexHull;
hull.ConvexRadius = convexRadius;
// Initialize blob arrays
{
byte *end = (byte *)collider + UnsafeUtility.SizeOf <ConvexCollider>();
hull.VerticesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.VerticesBlob.Offset));
hull.VerticesBlob.Length = tempHull.Vertices.Count;
end += sizeof(float3) * tempHull.Vertices.Count;
end = (byte *)Math.NextMultipleOf16((ulong)end); // planes currently must be aligned for Havok
hull.FacePlanesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FacePlanesBlob.Offset));
hull.FacePlanesBlob.Length = tempHull.Planes.Count;
end += sizeof(Plane) * tempHull.Planes.Count;
hull.FacesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FacesBlob.Offset));
hull.FacesBlob.Length = tempHull.Faces.Count;
end += sizeof(ConvexHull.Face) * tempHull.Faces.Count;
hull.FaceVertexIndicesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FaceVertexIndicesBlob.Offset));
hull.FaceVertexIndicesBlob.Length = tempHull.FaceVertexIndices.Count;
end += sizeof(byte) * tempHull.FaceVertexIndices.Count;
hull.VertexEdgesBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.VertexEdgesBlob.Offset));
hull.VertexEdgesBlob.Length = tempHull.VertexEdges.Count;
end += sizeof(ConvexHull.Edge) * tempHull.VertexEdges.Count;
hull.FaceLinksBlob.Offset = (int)(end - (byte *)UnsafeUtility.AddressOf(ref hull.FaceLinksBlob.Offset));
hull.FaceLinksBlob.Length = tempHull.FaceLinks.Count;
end += sizeof(ConvexHull.Edge) * tempHull.FaceLinks.Count;
}
// Fill blob arrays
{
for (int i = 0; i < tempHull.Vertices.Count; i++)
{
hull.Vertices[i] = tempHull.Vertices[i];
hull.VertexEdges[i] = tempHull.VertexEdges[i];
}
for (int i = 0; i < tempHull.Faces.Count; i++)
{
hull.Planes[i] = tempHull.Planes[i];
hull.Faces[i] = tempHull.Faces[i];
}
for (int i = 0; i < tempHull.FaceVertexIndices.Count; i++)
{
hull.FaceVertexIndices[i] = tempHull.FaceVertexIndices[i];
hull.FaceLinks[i] = tempHull.FaceLinks[i];
}
}
// Fill mass properties
{
var massProperties = builder.ComputeMassProperties();
Math.DiagonalizeSymmetricApproximation(massProperties.InertiaTensor, out float3x3 orientation, out float3 inertia);
float maxLengthSquared = 0.0f;
foreach (float3 vertex in hull.Vertices)
{
maxLengthSquared = math.max(maxLengthSquared, math.lengthsq(vertex - massProperties.CenterOfMass));
}
collider->MassProperties = new MassProperties
{
MassDistribution = new MassDistribution
{
Transform = new RigidTransform(orientation, massProperties.CenterOfMass),
InertiaTensor = inertia
},
Volume = massProperties.Volume,
AngularExpansionFactor = math.sqrt(maxLengthSquared)
};
}
}
// Write a set of contact manifolds for a pair of bodies to the given stream.
public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB,
float collisionTolerance, float timeStep, BodyIndexPair pair, ref NativeStream.Writer contactWriter)
{
Collider *colliderA = rigidBodyA.Collider;
Collider *colliderB = rigidBodyB.Collider;
if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter))
{
return;
}
// Build combined motion expansion
MotionExpansion expansion;
{
MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep);
MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep);
expansion = new MotionExpansion
{
Linear = expansionA.Linear - expansionB.Linear,
Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance
};
}
var context = new Context
{
BodyIndices = pair,
BodyCustomTags = new CustomTagsPair {
CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags
},
BodiesHaveInfiniteMass =
!math.any(motionVelocityA.InverseInertiaAndMass) &&
!math.any(motionVelocityB.InverseInertiaAndMass),
ContactWriter = (NativeStream.Writer *)UnsafeUtility.AddressOf(ref contactWriter)
};
var worldFromA = new MTransform(rigidBodyA.WorldFromBody);
var worldFromB = new MTransform(rigidBodyB.WorldFromBody);
// Dispatch to appropriate manifold generator
switch (colliderA->CollisionType)
{
case CollisionType.Convex:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Composite:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Composite:
CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Terrain:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Terrain:
UnityEngine.Assertions.Assert.IsTrue(false);
break;
}
break;
}
}
internal static unsafe void ExecuteImpl(int2 pair, Tree dynamicTree, ref NativeStream.Writer pairWriter)
{
var bodyFilters = (CollisionFilter *)dynamicTree.BodyFilters.GetUnsafeReadOnlyPtr();
var bodyRespondsToCollision = (bool *)dynamicTree.RespondsToCollision.GetUnsafeReadOnlyPtr();
var bufferedPairs = new BodyPairWriter((NativeStream.Writer *)UnsafeUtility.AddressOf(ref pairWriter), bodyFilters, bodyFilters, bodyRespondsToCollision, bodyRespondsToCollision, 0, 0);
new BoundingVolumeHierarchy(dynamicTree.Nodes, dynamicTree.NodeFilters).SelfBvhOverlap(ref bufferedPairs, pair.x, pair.y);
bufferedPairs.Close();
}
void ProcessSmallRange(Range baseRange, ref int freeNodeIndex)
{
Range range = baseRange;
ComputeAxisAndPivot(ref range, out int axis, out float pivot);
SortRange(axis, ref range);
Range *subRanges = stackalloc Range[4];
int hasLeftOvers = 1;
do
{
int numSubRanges = 0;
while (range.Length > 4 && numSubRanges < 3)
{
subRanges[numSubRanges].Start = range.Start;
subRanges[numSubRanges].Length = 4;
numSubRanges++;
range.Start += 4;
range.Length -= 4;
}
if (range.Length > 0)
{
subRanges[numSubRanges].Start = range.Start;
subRanges[numSubRanges].Length = range.Length;
numSubRanges++;
}
hasLeftOvers = 0;
CreateChildren(subRanges, numSubRanges, range.Root, ref freeNodeIndex, (Range *)UnsafeUtility.AddressOf(ref range), ref hasLeftOvers);
Assert.IsTrue(hasLeftOvers <= 1 /*, "Internal error"*/);
} while (hasLeftOvers > 0);
}
public unsafe static void Execute(ref JacobiansJobData <T> jobData, IntPtr additionalData,
IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex)
{
ModifiableJacobianHeader modifiableHeader;
ModifiableContactJacobian modifiableContact;
byte *headerBuffer = stackalloc byte[JacobianHeader.CalculateSize(JacobianType.Contact, (JacobianFlags)0xff, 4)]; //<todo.eoin.modifier How to verify correct sizes?
byte *contactBuffer = stackalloc byte[sizeof(ContactJacobian)];
Havok.Physics.HpGrid *curGrid = jobData.FixedJacobianGrid;
int *pluginIndexToLocal = jobData.PluginIndexToLocal->Data;
for (int g = 0; g < 2; g++)
{
for (int i = 0; i < curGrid->m_size; i++)
{
HpCsContactJacRange *gridRange = curGrid->m_entries + i;
var range = (Havok.Physics.HpLinkedRange *)UnsafeUtility.AddressOf(ref gridRange->m_range);
while (range != null)
{
var reader = new Havok.Physics.HpBlockStreamReader(range);
while (reader.HasItems)
{
var hpHeader = (Havok.Physics.HpJacHeader *)reader.Peek();
modifiableHeader = new ModifiableJacobianHeader {
};
modifiableContact = new ModifiableContactJacobian {
};
modifiableHeader.m_Header = (JacobianHeader *)headerBuffer;
modifiableContact.m_ContactJacobian = (ContactJacobian *)contactBuffer;
int bodyIndexA = pluginIndexToLocal[hpHeader->m_bodyIdA & 0x00ffffff];
int bodyIndexB = pluginIndexToLocal[hpHeader->m_bodyIdB & 0x00ffffff];
modifiableHeader.m_Header->BodyPair = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
modifiableHeader.EntityPair = new EntityPair
{
EntityA = jobData.Bodies[bodyIndexA].Entity,
EntityB = jobData.Bodies[bodyIndexB].Entity
};
modifiableHeader.m_Header->Type = JacobianType.Contact;
modifiableContact.m_ContactJacobian->BaseJacobian.NumContacts = hpHeader->m_numPoints;
modifiableContact.m_ContactJacobian->BaseJacobian.Normal = hpHeader->m_normal.xyz;
Havok.Physics.HPManifoldCollisionCache *manifoldCache = hpHeader->m_manifoldCollisionCache;
modifiableContact.m_ContactJacobian->CoefficientOfFriction = manifoldCache->m_friction.Value;
// Fill in friction data
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
modifiableContact.m_ContactJacobian->Friction0.AngularA = jf->m_jacDir0_angular0.xyz;
modifiableContact.m_ContactJacobian->Friction0.AngularB = jf->m_jacDir0_angular1.xyz;
modifiableContact.m_ContactJacobian->Friction1.AngularA = jf->m_jacDir1_angular0.xyz;
modifiableContact.m_ContactJacobian->Friction1.AngularB = jf->m_jacDir1_angular1.xyz;
modifiableContact.m_ContactJacobian->AngularFriction.AngularA = jf->m_jacAng_angular0.xyz;
modifiableContact.m_ContactJacobian->AngularFriction.AngularB = jf->m_jacAng_angular1.xyz;
}
Havok.Physics.HpPerManifoldProperty *cdp = manifoldCache->GetCustomPropertyStorage();
modifiableHeader.m_Header->Flags = (JacobianFlags)cdp->m_jacobianFlags;
if ((cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
modifiableHeader.MassFactors = *hpHeader->accessMassFactors();
}
for (int p = 0; p < hpHeader->m_numPoints; p++)
{
Havok.Physics.HpJacAngular *hpAng = hpHeader->accessJacAngular(p);
var ang = new ContactJacAngAndVelToReachCp
{
Jac = new ContactJacobianAngular
{
AngularA = hpAng->m_angular0.xyz,
AngularB = hpAng->m_angular1.xyz,
EffectiveMass = hpAng->m_angular0.w,
},
VelToReachCp = hpAng->m_angular1.w,
};
// Access the angular jacobian from the header directly,
// to avoid the modifiable header marking itself dirty.
modifiableHeader.m_Header->AccessAngularJacobian(p) = ang;
}
jobData.UserJobData.Execute(ref modifiableHeader, ref modifiableContact);
if (((byte)modifiableHeader.Flags & (byte)JacobianFlags.Disabled) != 0)
{
// Don't check the "changed" state of the jacobian - this flag is set on the contact
hpHeader->m_flagsAndDimB |= 1 << 10; // JH_MANIFOLD_IS_NOT_NORMAL
hpHeader->m_manifoldType = 3; // hknpManifoldType::DISABLED
}
if (modifiableHeader.AngularChanged || modifiableHeader.ModifiersChanged)
{
// Need to disable jacobian caching, since we can't tell what modifications the user has done
manifoldCache->m_qualityFlags &= (0xffff ^ (1 << 10)); //hknpBodyQuality::ENABLE_CONTACT_CACHING
}
if (modifiableHeader.AngularChanged)
{
for (int p = 0; p < hpHeader->m_numPoints; p++)
{
Havok.Physics.HpJacAngular * hpAng = hpHeader->accessJacAngular(p);
ContactJacAngAndVelToReachCp ang = modifiableHeader.GetAngularJacobian(p);
hpAng->m_angular0 = new float4(ang.Jac.AngularA, ang.Jac.EffectiveMass);
hpAng->m_angular1 = new float4(ang.Jac.AngularB, ang.VelToReachCp);
}
}
if (modifiableHeader.ModifiersChanged && (cdp->m_jacobianFlags & (byte)JacobianFlags.EnableMassFactors) != 0)
{
*hpHeader->accessMassFactors() = modifiableHeader.MassFactors;
}
if (modifiableHeader.ModifiersChanged && (cdp->m_jacobianFlags & (byte)JacobianFlags.EnableSurfaceVelocity) != 0)
{
var surfVel = modifiableHeader.SurfaceVelocity;
float angVelProj = math.dot(surfVel.AngularVelocity, modifiableContact.Normal);
if (manifoldCache != null)
{
float frictionRhs = manifoldCache->getFrictionRhsMultiplierValue();
float frictRhsMul = frictionRhs * jobData.TimeStep;
// Update cached integrated friction rhs
float4 vel = new float4(-surfVel.LinearVelocity, -angVelProj);
float4 rhs4 = manifoldCache->m_integratedFrictionRhs;
rhs4 += frictRhsMul * vel;
manifoldCache->m_integratedFrictionRhs = rhs4;
}
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Math.CalculatePerpendicularNormalized(modifiableContact.Normal, out float3 dir0, out float3 dir1);
float linVel0 = math.dot(surfVel.LinearVelocity, dir0);
float linVel1 = math.dot(surfVel.LinearVelocity, dir1);
// Check JH_SURFACE_VELOCITY_DIRTY flag and clear it if it was set
const ushort jhSurfaceVelocityDirty = 1 << 3;
if ((hpHeader->m_flagsAndDimB & jhSurfaceVelocityDirty) != 0)
{
*hpHeader->accessSurfaceVelocity() = new float3(linVel0, linVel1, angVelProj);
hpHeader->m_flagsAndDimB &= 0xffff ^ jhSurfaceVelocityDirty;
}
else
{
*hpHeader->accessSurfaceVelocity() += new float3(linVel0, linVel1, angVelProj);
}
// Update friction Jacobian
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
float dRhs0 = jf->m_jacDir0_linear0.w;
float dRhs1 = jf->m_jacDir1_linear0.w;
float dRhs = jf->m_jacAng_angular1.w;
float frictRhsMul = hpHeader->m_manifoldCollisionCache->getFrictionRhsMultiplierValue();
dRhs0 -= frictRhsMul * linVel0;
dRhs1 -= frictRhsMul * linVel1;
dRhs -= frictRhsMul * angVelProj;
jf->m_jacDir0_linear0.w = dRhs0;
jf->m_jacDir1_linear0.w = dRhs1;
jf->m_jacAng_angular1.w = dRhs;
}
}
}
if (modifiableContact.Modified)
{
hpHeader->m_normal.xyz = modifiableContact.Normal;
manifoldCache->m_friction.Value = modifiableContact.CoefficientOfFriction;
if (HpJacHeader.hasAnyFriction(hpHeader->m_flagsAndDimB))
{
Havok.Physics.HpJac3dFriction *jf = hpHeader->accessJacFriction();
jf->m_jacDir0_angular0.xyz = modifiableContact.Friction0.AngularA;
jf->m_jacDir0_angular1.xyz = modifiableContact.Friction0.AngularB;
jf->m_jacDir1_angular0.xyz = modifiableContact.Friction1.AngularA;
jf->m_jacDir1_angular1.xyz = modifiableContact.Friction1.AngularB;
jf->m_jacAng_angular0.xyz = modifiableContact.AngularFriction.AngularA;
jf->m_jacAng_angular1.xyz = modifiableContact.AngularFriction.AngularB;
}
}
reader.Advance(hpHeader->m_sizeDiv16 * 16);
}
range = range->m_next;
}
}
curGrid = jobData.MovingJacobianGrid;
}
}