// Create a joint asset with the given constraints
public static unsafe BlobAssetReference <JointData> Create(MTransform aFromJoint, MTransform bFromJoint, Constraint[] constraints)
{
// Allocate
int totalSize = sizeof(JointData) + sizeof(Constraint) * constraints.Length;
JointData *jointData = (JointData *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);
UnsafeUtility.MemClear(jointData, totalSize);
// Initialize
{
jointData->AFromJoint = aFromJoint;
jointData->BFromJoint = bFromJoint;
jointData->Version = 1;
byte *end = (byte *)jointData + sizeof(JointData);
jointData->m_ConstraintsBlob.Offset = UnsafeEx.CalculateOffset(end, ref jointData->m_ConstraintsBlob);
jointData->m_ConstraintsBlob.Length = constraints.Length;
for (int i = 0; i < constraints.Length; i++)
{
jointData->Constraints[i] = constraints[i];
}
}
var blob = BlobAssetReference <JointData> .Create(jointData, totalSize);
UnsafeUtility.Free(jointData, Allocator.Temp);
return(blob);
}
public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, uint numSubKeyBits, uint subKey)
{
m_ClosestHit.Transform(transform, numSubKeyBits, subKey);
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
if (m_MouseGroup.CalculateEntityCount() == 0)
{
return(inputDeps);
}
ComponentDataFromEntity <Translation> Positions = GetComponentDataFromEntity <Translation>(true);
ComponentDataFromEntity <Rotation> Rotations = GetComponentDataFromEntity <Rotation>(true);
ComponentDataFromEntity <PhysicsVelocity> Velocities = GetComponentDataFromEntity <PhysicsVelocity>();
ComponentDataFromEntity <PhysicsMass> Masses = GetComponentDataFromEntity <PhysicsMass>(true);
// If there's a pick job, wait for it to finish
if (m_PickSystem.PickJobHandle != null)
{
JobHandle.CombineDependencies(inputDeps, m_PickSystem.PickJobHandle.Value).Complete();
}
// If there's a picked entity, drag it
MousePickSystem.SpringData springData = m_PickSystem.SpringDatas[0];
if (springData.Dragging != 0)
{
Entity entity = m_PickSystem.SpringDatas[0].Entity;
if (!EntityManager.HasComponent <PhysicsMass>(entity))
{
return(inputDeps);
}
PhysicsMass massComponent = Masses[entity];
PhysicsVelocity velocityComponent = Velocities[entity];
if (massComponent.InverseMass == 0)
{
return(inputDeps);
}
var worldFromBody = new MTransform(Rotations[entity].Value, Positions[entity].Value);
// Body to motion transform
var bodyFromMotion = new MTransform(Masses[entity].InertiaOrientation, Masses[entity].CenterOfMass);
MTransform worldFromMotion = Mul(worldFromBody, bodyFromMotion);
// Damp the current velocity
const float gain = 0.95f;
velocityComponent.Linear *= gain;
velocityComponent.Angular *= gain;
// Get the body and mouse points in world space
float3 pointBodyWs = Mul(worldFromBody, springData.PointOnBody);
float3 pointSpringWs = Camera.main.ScreenToWorldPoint(new Vector3(Input.mousePosition.x, Input.mousePosition.y, springData.MouseDepth));
// Calculate the required change in velocity
float3 pointBodyLs = Mul(Inverse(bodyFromMotion), springData.PointOnBody);
float3 deltaVelocity;
{
float3 pointDiff = pointBodyWs - pointSpringWs;
float3 relativeVelocityInWorld = velocityComponent.Linear + math.mul(worldFromMotion.Rotation, math.cross(velocityComponent.Angular, pointBodyLs));
const float elasticity = 0.1f;
const float damping = 0.5f;
deltaVelocity = -pointDiff * (elasticity / Time.fixedDeltaTime) - damping * relativeVelocityInWorld;
}
// Build effective mass matrix in world space
// TODO how are bodies with inf inertia and finite mass represented
// TODO the aggressive damping is hiding something wrong in this code if dragging non-uniform shapes
float3x3 effectiveMassMatrix;
{
float3 arm = pointBodyWs - worldFromMotion.Translation;
var skew = new float3x3(
new float3(0.0f, arm.z, -arm.y),
new float3(-arm.z, 0.0f, arm.x),
new float3(arm.y, -arm.x, 0.0f)
);
// world space inertia = worldFromMotion * inertiaInMotionSpace * motionFromWorld
var invInertiaWs = new float3x3(
massComponent.InverseInertia.x * worldFromMotion.Rotation.c0,
massComponent.InverseInertia.y * worldFromMotion.Rotation.c1,
massComponent.InverseInertia.z * worldFromMotion.Rotation.c2
);
invInertiaWs = math.mul(invInertiaWs, math.transpose(worldFromMotion.Rotation));
float3x3 invEffMassMatrix = math.mul(math.mul(skew, invInertiaWs), skew);
invEffMassMatrix.c0 = new float3(massComponent.InverseMass, 0.0f, 0.0f) - invEffMassMatrix.c0;
invEffMassMatrix.c1 = new float3(0.0f, massComponent.InverseMass, 0.0f) - invEffMassMatrix.c1;
invEffMassMatrix.c2 = new float3(0.0f, 0.0f, massComponent.InverseMass) - invEffMassMatrix.c2;
effectiveMassMatrix = math.inverse(invEffMassMatrix);
}
// Calculate impulse to cause the desired change in velocity
float3 impulse = math.mul(effectiveMassMatrix, deltaVelocity);
// Clip the impulse
const float maxAcceleration = 250.0f;
float maxImpulse = math.rcp(massComponent.InverseMass) * Time.fixedDeltaTime * maxAcceleration;
impulse *= math.min(1.0f, math.sqrt((maxImpulse * maxImpulse) / math.lengthsq(impulse)));
// Apply the impulse
{
velocityComponent.Linear += impulse * massComponent.InverseMass;
float3 impulseLs = math.mul(math.transpose(worldFromMotion.Rotation), impulse);
float3 angularImpulseLs = math.cross(pointBodyLs, impulseLs);
velocityComponent.Angular += angularImpulseLs * massComponent.InverseInertia;
}
// Write back velocity
Velocities[entity] = velocityComponent;
}
return(inputDeps);
}
public void Transform(MTransform transform, int rigidBodyIndex)
{
Position = Mul(transform, Position);
SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal);
RigidBodyIndex = rigidBodyIndex;
}
public static unsafe Result ConvexConvex(ref ConvexHull convexA, ref ConvexHull convexB, MTransform aFromB)
{
return(ConvexConvex(
convexA.VerticesPtr, convexA.NumVertices, convexA.ConvexRadius,
convexB.VerticesPtr, convexB.NumVertices, convexB.ConvexRadius,
aFromB));
}
public static unsafe Result BoxSphere(BoxCollider *boxA, SphereCollider *sphereB, MTransform aFromB)
{
MTransform aFromBoxA = new MTransform(boxA->Orientation, boxA->Center);
float3 posBinA = Mul(aFromB, sphereB->Center);
float3 posBinBoxA = Mul(Inverse(aFromBoxA), posBinA);
float3 innerHalfExtents = boxA->Size * 0.5f - boxA->ConvexRadius;
float3 normalInBoxA;
float distance;
{
// from hkAabb::signedDistanceToPoint(), can optimize a lot
float3 projection = math.min(posBinBoxA, innerHalfExtents);
projection = math.max(projection, -innerHalfExtents);
float3 difference = projection - posBinBoxA;
float distanceSquared = math.lengthsq(difference);
// Check if the sphere center is inside the box
if (distanceSquared < 1e-6f)
{
float3 projectionLocal = projection;
float3 absProjectionLocal = math.abs(projectionLocal);
float3 del = absProjectionLocal - innerHalfExtents;
int axis = IndexOfMaxComponent(new float4(del, -float.MaxValue));
switch (axis)
{
case 0: normalInBoxA = new float3(projectionLocal.x < 0.0f ? 1.0f : -1.0f, 0.0f, 0.0f); break;
case 1: normalInBoxA = new float3(0.0f, projectionLocal.y < 0.0f ? 1.0f : -1.0f, 0.0f); break;
case 2: normalInBoxA = new float3(0.0f, 0.0f, projectionLocal.z < 0.0f ? 1.0f : -1.0f); break;
default:
normalInBoxA = new float3(1, 0, 0);
Assert.IsTrue(false);
break;
}
distance = math.max(del.x, math.max(del.y, del.z));
}
else
{
float invDistance = math.rsqrt(distanceSquared);
normalInBoxA = difference * invDistance;
distance = distanceSquared * invDistance;
}
}
float3 normalInA = math.mul(aFromBoxA.Rotation, normalInBoxA);
return(new Result
{
NormalInA = normalInA,
PositionOnAinA = posBinA + normalInA * (distance - boxA->ConvexRadius),
Distance = distance - (sphereB->Radius + boxA->ConvexRadius)
});
}
public void Transform(MTransform transform, uint numSubKeyBits, uint subKey)
{
Position = Mul(transform, Position);
SurfaceNormal = math.mul(transform.Rotation, SurfaceNormal);
ColliderKey.PushSubKey(numSubKeyBits, subKey);
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
bool success = false;
if (a.NumVertices + b.NumVertices < 64) // too slow without burst
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
ConvexHullBuilderStorage diffStorage = new ConvexHullBuilderStorage(maxNumVertices, Allocator.Temp, aabb, 0.0f, ConvexHullBuilder.IntResolution.Low);
ref ConvexHullBuilder diff = ref diffStorage.Builder;
success = true;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16));
}
}
float distance = 0.0f;
if (success && diff.Dimension == 3)
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
foreach (int t in diff.Triangles.Indices)
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
else
{
success = false;
}
diffStorage.Dispose();
if (success)
{
return(distance);
}
}
static void DrawManifold(bool useExerimentalMethod, ConvexConvexDistanceQueries.Result distance,
MTransform trsA, ref ConvexHullBuilder hullA, ref HullFaceData dataA,
MTransform trsB, ref ConvexHullBuilder hullB, ref HullFaceData dataB)
{
MTransform btoA = Mul(Inverse(trsA), trsB);
MTransform atoB = Mul(Inverse(trsB), trsA);
Face faceA = SelectBestFace(dataA.Faces, distance.ClosestPoints.NormalInA);
Face faceB = SelectBestFace(dataB.Faces, math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA));
if (useExerimentalMethod)
{
var legacyFaceA = faceA;
var legacyFaceB = faceB;
// Experimental method.
// Extract faces sub-set of A.
{
float bestDot = -2;
var normal = distance.ClosestPoints.NormalInA;
for (int i = 0; i < distance.SimplexDimension; ++i)
{
int vi = distance.SimplexVertexA(i);
for (int j = 0; j < dataA.VertexFaces[vi].NumFaces; ++j)
{
var k = dataA.FaceIndices[dataA.VertexFaces[vi].FirstFace + j];
var face = dataA.Faces[k];
var d = math.dot(face.Plane.Normal, normal);
if (d > bestDot)
{
faceA = face;
bestDot = d;
}
}
}
}
// Extract faces sub-set of B.
{
float bestDot = -2;
var normal = math.mul(atoB.Rotation, -distance.ClosestPoints.NormalInA);
for (int i = 0; i < distance.SimplexDimension; ++i)
{
int vi = distance.SimplexVertexB(i);
for (int j = 0; j < dataB.VertexFaces[vi].NumFaces; ++j)
{
var k = dataB.FaceIndices[dataB.VertexFaces[vi].FirstFace + j];
var face = dataB.Faces[k];
var d = math.dot(face.Plane.Normal, normal);
if (d > bestDot)
{
faceB = face;
bestDot = d;
}
}
}
}
if (legacyFaceA.FirstVertex != faceA.FirstVertex || legacyFaceB.FirstVertex != faceB.FirstVertex)
{
Debug.LogError("Different face set found.");
}
}
var facePlaneAinA = faceA.Plane;
var facePlaneBinA = TransformPlane(btoA, faceB.Plane);
/*drawFace(trsA, ref hullA, faceA, dataA.faceVertices, 0.01f, Color.yellow);
* drawFace(trsB, ref hullB, faceB, dataB.faceVertices, 0.01f, Color.yellow);*/
const float eps = 1e-6f;
var va = new List <float3>();
var vb = new List <float3>();
for (int i = 0; i < faceA.NumVertices; ++i)
{
va.Add(hullA.Vertices[dataA.FaceVertices[faceA.FirstVertex + i]].Position);
}
for (int i = 0; i < faceB.NumVertices; ++i)
{
vb.Add(Mul(btoA, hullB.Vertices[dataB.FaceVertices[faceB.FirstVertex + i]].Position));
}
var vt = new List <float3>();
for (int ai = va.Count - 1, aj = 0; aj < va.Count; ai = aj++)
{
var plane = new float4(math.normalize(math.cross(distance.ClosestPoints.NormalInA, va[ai] - va[aj])), 0);
plane.w = -math.dot(plane.xyz, va[ai]);
if (vb.Count > 1)
{
int bi = vb.Count - 1;
float d2Pi = DistanceToPlaneEps(plane, vb[bi], eps);
for (int bj = 0; bj < vb.Count; bi = bj++)
{
var d2Pj = DistanceToPlaneEps(plane, vb[bj], eps);
if ((d2Pi * d2Pj) < 0)
{
float isec = d2Pi / (d2Pi - d2Pj);
vt.Add(vb[bi] + (vb[bj] - vb[bi]) * isec);
}
if (d2Pj <= 0)
{
vt.Add(vb[bj]);
}
d2Pi = d2Pj;
}
}
var temp = vb;
vb = vt;
vt = temp;
vt.Clear();
}
if (vb.Count == 0)
{
vb.Add(distance.ClosestPoints.PositionOnBinA);
}
{
GUIStyle labelStyle = new GUIStyle();
labelStyle.fontSize = 16;
var projectionInvDen = 1 / math.dot(distance.ClosestPoints.NormalInA, facePlaneAinA.Normal);
int i = vb.Count - 1;
var piOnB = vb[i];
var piD = math.dot(facePlaneAinA, new float4(piOnB, 1)) * projectionInvDen;
var piOnA = piOnB - distance.ClosestPoints.NormalInA * piD;
for (int j = 0; j < vb.Count; i = j++)
{
#if UNITY_EDITOR
var center = Mul(trsA, (piOnA + piOnB) / 2);
Handles.Label(center, $"{piD}", labelStyle);
#endif
var pjOnB = vb[j];
var pjD = math.dot(facePlaneAinA, new float4(pjOnB, 1)) * projectionInvDen;
var pjOnA = pjOnB - distance.ClosestPoints.NormalInA * pjD;
Gizmos.DrawLine(Mul(trsA, piOnB), Mul(trsA, pjOnB));
Gizmos.DrawLine(Mul(trsA, piOnA), Mul(trsA, pjOnA));
Gizmos.DrawLine(Mul(trsA, pjOnB), Mul(trsA, pjOnA));
piOnB = pjOnB;
piOnA = pjOnA;
piD = pjD;
}
}
}
/// <summary>
/// Transforms a rotation from global space into the local space of the MTransform
/// </summary>
/// <param name="transform"></param>
/// <param name="globalRotation"></param>
/// <returns></returns>
public static MQuaternion InverseTransformRotation(this MTransform transform, MQuaternion globalRotation)
{
return(MQuaternionExtensions.Inverse(transform.Rotation).Multiply(globalRotation));
}
static void ValidateDistanceResult(DistanceQueries.Result result, ref ConvexHull a, ref ConvexHull b, MTransform aFromB, float referenceDistance, string failureMessage)
{
// Calculate the support distance along the separating normal
float3 tempA = getSupport(ref a, -result.NormalInA);
float3 supportQuery = tempA - result.NormalInA * a.ConvexRadius;
float3 tempB = Math.Mul(aFromB, getSupport(ref b, math.mul(aFromB.InverseRotation, result.NormalInA)));
float3 supportTarget = tempB + result.NormalInA * b.ConvexRadius;
float supportQueryDot = math.dot(supportQuery, result.NormalInA);
float supportTargetDot = math.dot(supportTarget, result.NormalInA);
float supportDistance = supportQueryDot - supportTargetDot;
// Increase the tolerance in case of core penetration
float adjustedTolerance = tolerance;
float zeroCoreDistance = -a.ConvexRadius - b.ConvexRadius; // Distance of shapes including radius at which core shapes are penetrating
if (result.Distance < zeroCoreDistance || referenceDistance < zeroCoreDistance || supportDistance < zeroCoreDistance)
{
// Core shape penetration distances are less accurate, and error scales with the number of vertices (as well as shape size). See stopThreshold in ConvexConvexDistanceQueries.
// This is not usually noticeable in rigid body simulation because accuracy improves as the penetration resolves.
// The tolerance is tuned for these tests, it might require further tuning as the tests change.
adjustedTolerance = 1e-2f + 1e-3f * (a.NumVertices + b.NumVertices);
}
// Check that the distance is consistent with the reference distance
Assert.AreEqual(result.Distance, referenceDistance, adjustedTolerance, failureMessage + ": incorrect distance");
// Check that the separating normal and closest point are consistent with the distance
Assert.AreEqual(result.Distance, supportDistance, adjustedTolerance, failureMessage + ": incorrect normal");
float positionDot = math.dot(result.PositionOnAinA, result.NormalInA);
Assert.AreEqual(supportQueryDot, positionDot, adjustedTolerance, failureMessage + ": incorrect position");
}
/// <summary>
/// Transforms a rotation form local space of the MTransform to the global space
/// </summary>
/// <param name="transform"></param>
/// <param name="localRotation"></param>
/// <returns></returns>
public static MQuaternion TransformRotation(this MTransform transform, MQuaternion localRotation)
{
return(transform.Rotation.Multiply(localRotation));
}
/// <summary>
/// Transforms a point from the global space to the local space of the MTransform
/// </summary>
/// <param name="transform"></param>
/// <param name="globalPosition"></param>
/// <returns></returns>
public static MVector3 InverseTransformPoint(this MTransform transform, MVector3 globalPosition)
{
return(MQuaternionExtensions.Inverse(transform.Rotation).Multiply(globalPosition.Subtract(transform.Position)));
}
/// <summary>
/// Transforms a point from the local space of the MTransform to the global space
/// </summary>
/// <param name="transform"></param>
/// <param name="localPosition"></param>
/// <returns></returns>
public static MVector3 TransformPoint(this MTransform transform, MVector3 localPosition)
{
return(transform.Rotation.Multiply(localPosition).Add(transform.Position));
}
/// <summary>
/// Creates a Unity collider based on the MCollider and a transform
/// </summary>
/// <param name="collider"></param>
/// <param name="transform"></param>
/// <returns></returns>
public static Collider CreateCollider(MCollider collider, MTransform transform)
{
if (collider == null || transform == null)
{
return(null);
}
GameObject result = null;
switch (collider.Type)
{
case MColliderType.Box:
MBoxColliderProperties mboxCollider = collider.BoxColliderProperties;
if (mboxCollider == null)
{
Debug.Log("Box collider is null");
return(null);
}
GameObject boxGameObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
boxGameObject.transform.position = transform.Position.ToVector3();
boxGameObject.transform.rotation = transform.Rotation.ToQuaternion();
//Assign the properties of the collider
BoxCollider boxCollider = boxGameObject.GetComponent <BoxCollider>();
boxCollider.center = collider.PositionOffset.ToVector3();
boxCollider.size = mboxCollider.Size.ToVector3();
result = boxGameObject;
break;
case MColliderType.Sphere:
MSphereColliderProperties mSphereCollider = collider.SphereColliderProperties;
if (mSphereCollider == null)
{
Debug.Log("Sphere collider is null");
return(null);
}
GameObject sphereGameObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
sphereGameObject.transform.position = transform.Position.ToVector3();
sphereGameObject.transform.rotation = transform.Rotation.ToQuaternion();
SphereCollider sphereCollider = sphereGameObject.GetComponent <SphereCollider>();
sphereCollider.center = collider.PositionOffset.ToVector3();
sphereCollider.radius = (float)mSphereCollider.Radius;
result = sphereGameObject;
break;
case MColliderType.Capsule:
MCapsuleColliderProperties mCapsuleCollider = collider.CapsuleColliderProperties;
if (mCapsuleCollider == null)
{
Debug.Log("Capsule collider is null");
return(null);
}
GameObject capsule = GameObject.CreatePrimitive(PrimitiveType.Capsule);
capsule.transform.position = transform.Position.ToVector3();
capsule.transform.rotation = transform.Rotation.ToQuaternion();
CapsuleCollider capsuleCollider = capsule.GetComponent <CapsuleCollider>();
capsuleCollider.center = collider.PositionOffset.ToVector3();
capsuleCollider.radius = (float)mCapsuleCollider.Radius;
capsuleCollider.height = (float)mCapsuleCollider.Height;
result = capsule;
break;
case MColliderType.Mesh:
MMeshColliderProperties mMeshCollider = collider.MeshColliderProperties;
if (mMeshCollider == null)
{
Debug.Log("Mesh collider is null");
return(null);
}
//To do
break;
}
return(result.GetComponent <Collider>());
}
void OnDrawGizmos()
{
{
var s = 0.25f;
var com = MassProperties.CenterOfMass;
Gizmos.color = Color.white;
Gizmos.DrawLine(transform.TransformPoint(com + new float3(-s, 0, 0)), transform.TransformPoint(com + new float3(+s, 0, 0)));
Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, -s, 0)), transform.TransformPoint(com + new float3(0, +s, 0)));
Gizmos.DrawLine(transform.TransformPoint(com + new float3(0, 0, -s)), transform.TransformPoint(com + new float3(0, 0, +s)));
}
if (UpdateMesh)
{
UpdateMesh = false;
UpdateMeshNow();
}
// Display faces.
if (ShowFaces && HullData.Faces != null)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
foreach (Face face in HullData.Faces)
{
var offset = face.Plane.Normal * 0.0001f;
for (int i = face.NumVertices - 1, j = 0; j < face.NumVertices; i = j++)
{
var a = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + i]].Position;
var b = Hull.Vertices[HullData.FaceVertices[face.FirstVertex + j]].Position;
a = Mul(trs, a + offset);
b = Mul(trs, b + offset);
Gizmos.DrawLine(a, b);
}
}
}
// Display triangles.
if (ShowTriangles)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
for (int i = Hull.Triangles.GetFirstIndex(); i != -1; i = Hull.Triangles.GetNextIndex(i))
{
var a = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex0].Position);
var b = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex1].Position);
var c = Mul(trs, Hull.Vertices[Hull.Triangles[i].Vertex2].Position);
Gizmos.DrawLine(a, b);
Gizmos.DrawLine(b, c);
Gizmos.DrawLine(c, a);
}
}
// Display vertex normals.
if (ShowVertexNormals)
{
MTransform trs = new MTransform(transform.rotation, transform.position);
Gizmos.color = Color.white;
for (var vertex = Hull.Triangles.GetFirstIndex(); vertex != -1; vertex = Hull.Triangles.GetNextIndex(vertex))
{
var normal = math.mul(trs.Rotation, Hull.ComputeVertexNormal(vertex)) * 0.25f;
var start = Mul(trs, Hull.Vertices[vertex].Position);
Gizmos.DrawRay(start, normal);
}
}
// Display labels.
if (ShowLabels)
{
}
// Compute distance to every other convex hulls.
if (CollideOthers)
{
var thisId = GetInstanceID();
var cvxs = FindObjectsOfType <ConvexConvexDistanceTest>();
MTransform transformA = new MTransform(transform.rotation, transform.position);
float3[] verticesA = GetVertexArray();
foreach (var cvx in cvxs)
{
if (cvx.GetInstanceID() == thisId)
{
continue;
}
MTransform transformB = new MTransform(cvx.transform.rotation, cvx.transform.position);
float3[] verticesB = cvx.GetVertexArray();
MTransform btoA = Mul(Inverse(transformA), transformB);
ConvexConvexDistanceQueries.Result result;
fixed(float3 *va = verticesA)
{
fixed(float3 *vb = verticesB)
{
result = ConvexConvexDistanceQueries.ConvexConvex(va, verticesA.Length, vb, verticesB.Length, btoA, PenetrationHandling);
}
}
var from = Mul(transformA, result.ClosestPoints.PositionOnAinA);
var to = Mul(transformA, result.ClosestPoints.PositionOnBinA);
if (TraceQueryResults)
{
Debug.Log($"Iterations={result.Iterations}, plane={result.ClosestPoints.NormalInA}, distance={result.ClosestPoints.Distance}");
Debug.Log($"Features A = [{result.Simplex[0] >> 16}, {result.Simplex[1] >> 16}, {result.Simplex[2] >> 16}]");
Debug.Log($"Features B = [{result.Simplex[0] & 0xffff}, {result.Simplex[1] & 0xffff}, {result.Simplex[2] & 0xffff}]");
}
if (ShowManifold && Hull.Dimension == 3 && cvx.Hull.Dimension == 3)
{
DrawManifold(Experimental, result,
transformA, ref Hull, ref HullData,
transformB, ref cvx.Hull, ref cvx.HullData);
}
if (ShowProjection)
{
Gizmos.color = Color.white;
var trs = Mul(transformA, new MTransform(float3x3.identity, result.ClosestPoints.NormalInA * result.ClosestPoints.Distance));
{
var tv = stackalloc float3[Hull.Vertices.PeakCount];
for (var vertex = Hull.Vertices.GetFirstIndex(); vertex != -1; vertex = Hull.Vertices.GetNextIndex(vertex))
{
tv[vertex] = Mul(trs, Hull.Vertices[vertex].Position);
}
if (Hull.Dimension == 3)
{
for (var edge = Hull.GetFirstPrimaryEdge(); edge.IsValid; edge = Hull.GetNextPrimaryEdge(edge))
{
Gizmos.DrawLine(tv[Hull.StartVertex(edge)], tv[Hull.EndVertex(edge)]);
}
}
else if (Hull.Dimension >= 1)
{
for (int i = Hull.Vertices.PeakCount - 1, j = 0; j < Hull.Vertices.PeakCount; i = j++)
{
Gizmos.DrawLine(tv[i], tv[j]);
}
}
}
}
Gizmos.color = Color.red;
Gizmos.DrawSphere(from, 0.05f);
Gizmos.color = Color.green;
Gizmos.DrawSphere(to, 0.05f);
Gizmos.color = Color.white;
Gizmos.DrawLine(from, to);
if (ShowCso)
{
Gizmos.color = Color.yellow;
using (var cso = new ConvexHullBuilder(8192, 8192 * 2))
{
for (int i = 0; i < verticesA.Length; ++i)
{
for (int j = 0; j < verticesB.Length; ++j)
{
cso.AddPoint(verticesA[i] - Mul(btoA, verticesB[j]));
}
}
if (cso.Dimension == 2)
{
for (int n = cso.Vertices.PeakCount, i = n - 1, j = 0; j < n; i = j++)
{
Gizmos.DrawLine(cso.Vertices[i].Position, cso.Vertices[j].Position);
}
}
else if (cso.Dimension == 3)
{
foreach (var triangle in cso.Triangles.Elements)
{
Gizmos.DrawLine(cso.Vertices[triangle.Vertex0].Position, cso.Vertices[triangle.Vertex1].Position);
Gizmos.DrawLine(cso.Vertices[triangle.Vertex1].Position, cso.Vertices[triangle.Vertex2].Position);
Gizmos.DrawLine(cso.Vertices[triangle.Vertex2].Position, cso.Vertices[triangle.Vertex0].Position);
}
}
Gizmos.DrawLine(new float3(-0.1f, 0, 0), new float3(+0.1f, 0, 0));
Gizmos.DrawLine(new float3(0, -0.1f, 0), new float3(0, +0.1f, 0));
Gizmos.DrawLine(new float3(0, 0, -0.1f), new float3(0, 0, +0.1f));
}
}
}
}
// Draw vertices.
#if UNITY_EDITOR
GUIStyle labelStyle = new GUIStyle();
labelStyle.fontSize = 24;
#endif
Gizmos.color = Color.yellow;
for (int i = Hull.Vertices.GetFirstIndex(); i != -1; i = Hull.Vertices.GetNextIndex(i))
{
var w = transform.TransformPoint(Hull.Vertices[i].Position);
#if UNITY_EDITOR
if (ShowLabels)
{
Handles.color = Color.white;
Handles.Label(w, $"{i}:{Hull.Vertices[i].Cardinality}", labelStyle);
}
else
#endif
{
Gizmos.DrawSphere(w, 0.01f);
}
}
}
public static unsafe Result SphereSphere(SphereCollider *sphereA, SphereCollider *sphereB, MTransform aFromB)
{
float3 posBinA = Mul(aFromB, sphereB->Center);
return(PointPoint(sphereA->Center, posBinA, sphereA->Radius, sphereA->Radius + sphereB->Radius));
}
protected void RCS(Vector3 vTurnProp)
{
//angular acceleration = torque/mass
rates = torques / m_rigidbody.mass;
Vector3 vThrustProp = vTurnProp;
// //determine targer rates of rotation based on user input as a percentage of the maximum angular velocity
m_vTargetVelocity = new Vector3(vThrustProp.x * rates.x, vThrustProp.y * rates.y, vThrustProp.z * rates.z);
// //take the rigidbody.angularVelocity and convert it to local space; we need this for comparison to target rotation velocities
curVelocity = MTransform.InverseTransformDirection(m_rigidbody.angularVelocity);
//****************************************************************************************************************
//For each axis: If the ship's current rate of rotation does not equal the desired rate of rotation, first check to see
//if it is a matter of drift or "jittering", which is when it keeps jumping from positive to negative to positive thrust because the
//values are so close to zero (to see what I mean, set snapThreshold = 0, rotate the ship on multiple axes, then let it try
//to come to a complete stop. It won't.) If it is just drift/jittering, turn off the thrust for the axis, and just set the current
//angular velocity to the target angular velocity. Otherwise, the user is still giving input, and we haven't reached the
//desired rate of rotation. In that case, we set the axis activation value = to the direction in which we need thrust.
//****************************************************************************************************************
if (curVelocity.x != m_vTargetVelocity.x)
{
if (Mathf.Abs(m_vTargetVelocity.x - curVelocity.x) < rates.x * Time.fixedDeltaTime * snapThreshold)
{
tActivation.x = 0;
curVelocity.x = m_vTargetVelocity.x;
}
else
{
tActivation.x = Mathf.Sign(m_vTargetVelocity.x - curVelocity.x);
}
}
if (curVelocity.y != m_vTargetVelocity.y)
{
if (Mathf.Abs(m_vTargetVelocity.y - curVelocity.y) < rates.y * Time.fixedDeltaTime * snapThreshold)
{
tActivation.y = 0;
curVelocity.y = m_vTargetVelocity.y;
}
else
{
tActivation.y = Mathf.Sign(m_vTargetVelocity.y - curVelocity.y);
}
}
if (curVelocity.z != m_vTargetVelocity.z)
{
if (Mathf.Abs(m_vTargetVelocity.z - curVelocity.z) < rates.z * Time.fixedDeltaTime * snapThreshold)
{
tActivation.z = 0;
curVelocity.z = m_vTargetVelocity.z;
}
else
{
tActivation.z = Mathf.Sign(m_vTargetVelocity.z - curVelocity.z);
}
}
//here, we manually set the rigidbody.angular velocity to the value of our current velocity.
//this is done to effect the manual changes we may have made on any number of axes.
//if we didn't do this, the jittering would continue to occur.
if (InputManager.Instance.NoviceMode)
{
}
else
{
m_rigidbody.angularVelocity = MTransform.TransformDirection(curVelocity);
}
//call the function that actually handles applying the torque
FireThrusters(vTurnProp);
}
public static unsafe Result CapsuleCapsule(CapsuleCollider *capsuleA, CapsuleCollider *capsuleB, MTransform aFromB)
{
// Transform capsule B into A-space
float3 pointB = Mul(aFromB, capsuleB->Vertex0);
float3 edgeB = math.mul(aFromB.Rotation, capsuleB->Vertex1 - capsuleB->Vertex0);
// Get point and edge of A
float3 pointA = capsuleA->Vertex0;
float3 edgeA = capsuleA->Vertex1 - capsuleA->Vertex0;
// Get the closest points on the capsules
SegmentSegment(pointA, edgeA, pointB, edgeB, out float3 closestA, out float3 closestB);
float3 diff = closestA - closestB;
float coreDistanceSq = math.lengthsq(diff);
if (coreDistanceSq < distanceEpsSq)
{
// Special case for extremely small distances, should be rare
float3 normal = math.cross(edgeA, edgeB);
if (math.lengthsq(normal) < 1e-5f)
{
float3 edge = math.normalizesafe(edgeA, math.normalizesafe(edgeB, new float3(1, 0, 0))); // edges are parallel or one of the capsules is a sphere
Math.CalculatePerpendicularNormalized(edge, out normal, out float3 unused); // normal is anything perpendicular to edge
}
else
{
normal = math.normalize(normal); // normal is cross of edges, sign doesn't matter
}
return(new Result
{
NormalInA = normal,
PositionOnAinA = pointA - normal * capsuleA->Radius,
Distance = -capsuleA->Radius - capsuleB->Radius
});
}
return(PointPoint(closestA, closestB, capsuleA->Radius, capsuleA->Radius + capsuleB->Radius));
}
protected void FireThrusters(Vector3 vOrientationProp)
{
//for each axis, applies torque based on the torque available to the axis in the direction indicated by the activation value.
//-1 means we are applying torque to effect a negative rotation. +1 does just the opposite. 0 means no torque is needed.
Vector3 vThrustProp = vOrientationProp;
Vector3 vXTorque = Vector3.zero;
Vector3 vYTorque = Vector3.zero;
Vector3 vZTorque = Vector3.zero;
if (!InputManager.Instance.NoviceMode)
{
vXTorque = tActivation.x * MTransform.TransformDirection(Vector3.right) * torques.x * Mathf.Abs(vThrustProp.x) * Time.fixedDeltaTime;
vYTorque = tActivation.y * MTransform.TransformDirection(Vector3.up) * torques.y * Mathf.Abs(vThrustProp.y) * Time.fixedDeltaTime;
vZTorque = tActivation.z * MTransform.TransformDirection(Vector3.forward) * torques.z * Mathf.Abs(vThrustProp.z) * Time.fixedDeltaTime;
}
else
{
Vector3 vLocalRight = MTransform.TransformDirection(Vector3.right);
vLocalRight.y = 0;
vXTorque = tActivation.x * vLocalRight.normalized * torques.x * Mathf.Abs(vThrustProp.x) * Time.fixedDeltaTime;
vYTorque = tActivation.y * (Vector3.up) * torques.y * Mathf.Abs(vThrustProp.y) * Time.fixedDeltaTime;
}
if (!MLandingAbility.Landed)
{
Vector3 localVelocity = MTransform.InverseTransformDirection(m_rigidbody.velocity);
float ForwardSpeed = Mathf.Max(0, localVelocity.z);
Vector3 vTorque = Vector3.zero;
if (InputManager.Instance.NoviceMode)
{
Quaternion localRotation = MTransform.localRotation;
Quaternion axisRotation = Quaternion.AngleAxis(localRotation.eulerAngles[0], rotateAround);
float angleFromMin = Quaternion.Angle(axisRotation, minQuaternion);
float angleFromMax = Quaternion.Angle(axisRotation, maxQuaternion);
float fDotUp = Vector3.Dot(MTransform.forward, Vector3.up);
float fDotDown = Vector3.Dot(MTransform.forward, -Vector3.up);
bool bDiffUp = (1f - fDotUp) < 0.1f;
bool bDiffDown = (1f - fDotDown) < 0.1f;
bool bDiff = bDiffUp || bDiffDown;
bool bDownDirMovement = bDiffUp && !bDiffDown && tActivation.x > 0;
bool bUpDirMovement = bDiffDown && !bDiffUp && tActivation.x < 0;
if (!bDiff || bDownDirMovement || bUpDirMovement)
{
if (tActivation.x != 0)
{
vTorque += vXTorque;
}
}
else
{
Vector3 vAngular = m_rigidbody.angularVelocity;
vAngular.x = 0f;
vAngular.z = 0f;
m_rigidbody.angularVelocity = vAngular;
}
if (tActivation.y != 0)
{
vTorque += vYTorque;
}
}
else
{
if (tActivation.x != 0)
{
vTorque += vXTorque;
}
if (tActivation.y != 0)
{
vTorque += vYTorque;
}
if (tActivation.z != 0)
{
vTorque += vZTorque;
}
}
if (AllowTorquing && !InputManager.Instance.NoviceMode)
{
m_rigidbody.AddTorque(vTorque);
}
else if (AllowTorquing && InputManager.Instance.NoviceMode)
{
m_rigidbody.AddTorque(vTorque);
}
}
if (InputManager.Instance.IsMovement() && !m_playerShip.m_airBrakePress.IsActive)
{
SpeedStat.Accelerate();
if (MTransform.forward.y < 0f)
{
Vector3 vDownForce = Physics.gravity.y * GameSimulation.Instance.GravityMultiplier * MTransform.forward.y * MTransform.forward * GameSimulation.Instance.GravityMultiplier;
m_rigidbody.AddForce(vDownForce, ForceMode.Force);
}
}
else
{
SpeedStat.Decelerate();
}
if (!MLandingAbility.Landed)
{
SpeedStat.AddForce(vThrustProp.x, vThrustProp.y);
}
bool bDashCheck = m_playerShip.MDashAbility == null ||
(m_playerShip.MDashAbility != null && m_playerShip.MDashAbility.Cooldown.IsMin()) ||
(m_playerShip.MSpeedGateEffect.ForcesActive());
if (m_bAeroSteering)
{
if ((Rigidbody.velocity.magnitude > 0 && bDashCheck) || DisableAeroDynamic)
{
AeroDynamicEffect.ModifyCurrent(Time.fixedDeltaTime * m_fAeroGainMod);
// compare the direction we're pointing with the direction we're moving:
m_AeroFactor = Vector3.Dot(MTransform.forward, Rigidbody.velocity.normalized);
// multipled by itself results in a desirable rolloff curve of the effect
m_AeroFactor *= m_AeroFactor;
// Finally we calculate a new velocity by bending the current velocity direction towards
// the the direction the plane is facing, by an amount based on this aeroFactor
Vector3 newVelocity = Vector3.Lerp(Rigidbody.velocity, MTransform.forward * Rigidbody.velocity.magnitude,
m_AeroFactor * Rigidbody.velocity.magnitude * AeroDynamicEffect.Current * Time.fixedDeltaTime);
Rigidbody.velocity = newVelocity;
}
else
{
AeroDynamicEffect.SetToMin();
}
}
}
public static unsafe Result CapsuleTriangle(CapsuleCollider *capsuleA, PolygonCollider *triangleB, MTransform aFromB)
{
// Get vertices
float3 c0 = capsuleA->Vertex0;
float3 c1 = capsuleA->Vertex1;
float3 t0 = Mul(aFromB, triangleB->ConvexHull.Vertices[0]);
float3 t1 = Mul(aFromB, triangleB->ConvexHull.Vertices[1]);
float3 t2 = Mul(aFromB, triangleB->ConvexHull.Vertices[2]);
float3 direction;
float distanceSq;
float3 pointCapsule;
float sign = 1.0f; // negated if penetrating
{
// Calculate triangle edges and edge planes
float3 faceNormal = math.mul(aFromB.Rotation, triangleB->ConvexHull.Planes[0].Normal);
FourTransposedPoints vertsB;
FourTransposedPoints edgesB;
FourTransposedPoints perpsB;
CalcTrianglePlanes(t0, t1, t2, faceNormal, out vertsB, out edgesB, out perpsB);
// c0 against triangle face
{
FourTransposedPoints rels = new FourTransposedPoints(c0) - vertsB;
PointTriangleFace(c0, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance);
distanceSq = signedDistance * signedDistance;
if (distanceSq > distanceEpsSq)
{
direction = -faceNormal * signedDistance;
}
else
{
direction = math.select(faceNormal, -faceNormal, math.dot(c1 - c0, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face
}
pointCapsule = c0;
}
// c1 against triangle face
{
FourTransposedPoints rels = new FourTransposedPoints(c1) - vertsB;
PointTriangleFace(c1, t0, faceNormal, vertsB, edgesB, perpsB, rels, out float signedDistance);
float distanceSq1 = signedDistance * signedDistance;
float3 direction1;
if (distanceSq1 > distanceEpsSq)
{
direction1 = -faceNormal * signedDistance;
}
else
{
direction1 = math.select(faceNormal, -faceNormal, math.dot(c0 - c1, faceNormal) < 0); // rare case, capsule point is exactly on the triangle face
}
SelectMin(ref direction, ref distanceSq, ref pointCapsule, direction1, distanceSq1, c1);
}
// axis against triangle edges
float3 axis = c1 - c0;
for (int i = 0; i < 3; i++)
{
float3 closestOnCapsule, closestOnTriangle;
SegmentSegment(c0, axis, vertsB.GetPoint(i), edgesB.GetPoint(i), out closestOnCapsule, out closestOnTriangle);
float3 edgeDiff = closestOnCapsule - closestOnTriangle;
float edgeDistanceSq = math.lengthsq(edgeDiff);
edgeDiff = math.select(edgeDiff, perpsB.GetPoint(i), edgeDistanceSq < distanceEpsSq); // use edge plane if the capsule axis intersects the edge
SelectMin(ref direction, ref distanceSq, ref pointCapsule, edgeDiff, edgeDistanceSq, closestOnCapsule);
}
// axis against triangle face
{
// Find the intersection of the axis with the triangle plane
float axisDot = math.dot(axis, faceNormal);
float dist0 = math.dot(t0 - c0, faceNormal); // distance from c0 to the plane along the normal
float t = dist0 * math.select(math.rcp(axisDot), 0.0f, axisDot == 0.0f);
if (t > 0.0f && t < 1.0f)
{
// If they intersect, check if the intersection is inside the triangle
FourTransposedPoints rels = new FourTransposedPoints(c0 + axis * t) - vertsB;
float4 dots = perpsB.Dot(rels);
if (math.all(dots <= float4.zero))
{
// Axis intersects the triangle, choose the separating direction
float dist1 = axisDot - dist0;
bool use1 = math.abs(dist1) < math.abs(dist0);
float dist = math.select(-dist0, dist1, use1);
float3 closestOnCapsule = math.select(c0, c1, use1);
SelectMin(ref direction, ref distanceSq, ref pointCapsule, dist * faceNormal, dist * dist, closestOnCapsule);
// Even if the edge is closer than the face, we now know that the edge hit was penetrating
sign = -1.0f;
}
}
}
}
float invDistance = math.rsqrt(distanceSq);
float distance;
float3 normal;
if (distanceSq < distanceEpsSq)
{
normal = math.normalize(direction); // rare case, capsule axis almost exactly touches the triangle
distance = 0.0f;
}
else
{
normal = direction * invDistance * sign; // common case, distanceSq = lengthsq(direction)
distance = distanceSq * invDistance * sign;
}
return(new Result
{
NormalInA = normal,
PositionOnAinA = pointCapsule - normal * capsuleA->Radius,
Distance = distance - capsuleA->Radius
});
}
void OnCollisionEnter(Collision collision)
{
m_playerShip.OnCollisionHitForAbilities(collision);
MLandingAbility.MoverCollisionHitLogic(collision);
LockOnCamera.Instance.PlayShakeEvent(m_collideEventData);
for (int i = 0; i < collision.contacts.Length; ++i)
{
Vector3 vLoc = collision.contacts[i].point;
Collider hitCol = collision.contacts[i].thisCollider;
if (hitCol == GetComponent <Collider>())
{
if (MLandingAbility.Landing || MLandingAbility.Landed)
{
return;
}
bool bCollideDamageLayer = LayerMask.NameToLayer("Scenery") == collision.collider.gameObject.layer ||
LayerMask.NameToLayer("Building") == collision.collider.gameObject.layer;
//PlayEffect
if (bCollideDamageLayer)
{
m_collisionDamage.SetCollisionInfo(collision);
m_playerShip.HandleMod(m_collisionDamage);
m_playerShip.StealthMeter.ModifyCurrent(-Rigidbody.velocity.magnitude);
m_speedDamageMod.Amount = -m_playerShip.m_fSpeed;
LastCollisionVector = collision.impulse / Time.fixedDeltaTime;
collision.gameObject.SendMessage("OnRammedByPlayer", m_speedDamageMod, SendMessageOptions.DontRequireReceiver);
MasterAudio.PlaySound(m_sOnRegularHit);
}
else if (LayerMask.NameToLayer("Water") == collision.collider.gameObject.layer)
{
m_playerShip.StealthMeter.ModifyCurrent(-Rigidbody.velocity.magnitude);
Vector3 vAdded = m_playerShip.MTransform.forward;
vAdded.y = 0;
vAdded.Normalize();
vAdded *= 4f;
PoolBoss.Spawn(m_waterCollisionPrefab.transform, vLoc + vAdded, Quaternion.identity, null);
}
else if (LayerMask.NameToLayer("Actor") == collision.collider.gameObject.layer)
{
m_playerShip.StealthMeter.ModifyCurrent((m_collisionDamage.Amount));
}
else if (LayerMask.NameToLayer("FlockingSpirits") == collision.collider.gameObject.layer)
{
MasterAudio.PlaySound(m_sOnFlockingHit);
Transform mForm = PoolBoss.Spawn(m_impactEffect.transform, vLoc, Quaternion.identity, MTransform);
mForm.parent = MTransform;
mForm.localPosition = MTransform.InverseTransformPoint(vLoc);
}
else if (LayerMask.NameToLayer("Vehicle") == collision.collider.gameObject.layer)
{
MasterAudio.PlaySound(m_sOnVehicleHit);
Transform mForm = PoolBoss.Spawn(m_impactEffect.transform, vLoc, Quaternion.identity, MTransform);
mForm.parent = MTransform;
mForm.localPosition = MTransform.InverseTransformPoint(vLoc);
}
}
}
}
// Dispatch any pair of convex colliders
public static unsafe Result ConvexConvex(Collider *convexA, Collider *convexB, MTransform aFromB)
{
Result result;
bool flip = false;
switch (convexA->Type)
{
case ColliderType.Sphere:
SphereCollider *sphereA = (SphereCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
result = SphereSphere(sphereA, (SphereCollider *)convexB, aFromB);
break;
case ColliderType.Capsule:
CapsuleCollider *capsuleB = (CapsuleCollider *)convexB;
result = CapsuleSphere(capsuleB->Vertex0, capsuleB->Vertex1, capsuleB->Radius, sphereA->Center, sphereA->Radius, Inverse(aFromB));
flip = true;
break;
case ColliderType.Triangle:
PolygonCollider *triangleB = (PolygonCollider *)convexB;
result = TriangleSphere(
triangleB->Vertices[0], triangleB->Vertices[1], triangleB->Vertices[2], triangleB->Planes[0].Normal,
sphereA->Center, sphereA->Radius, Inverse(aFromB));
flip = true;
break;
case ColliderType.Quad:
PolygonCollider *quadB = (PolygonCollider *)convexB;
result = QuadSphere(
quadB->Vertices[0], quadB->Vertices[1], quadB->Vertices[2], quadB->Vertices[3], quadB->Planes[0].Normal,
sphereA->Center, sphereA->Radius, Inverse(aFromB));
flip = true;
break;
case ColliderType.Box:
result = BoxSphere((BoxCollider *)convexB, sphereA, Inverse(aFromB));
flip = true;
break;
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref sphereA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Capsule:
CapsuleCollider *capsuleA = (CapsuleCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
SphereCollider *sphereB = (SphereCollider *)convexB;
result = CapsuleSphere(capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB);
break;
case ColliderType.Capsule:
result = CapsuleCapsule(capsuleA, (CapsuleCollider *)convexB, aFromB);
break;
case ColliderType.Triangle:
result = CapsuleTriangle(capsuleA, (PolygonCollider *)convexB, aFromB);
break;
case ColliderType.Box:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref capsuleA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Triangle:
PolygonCollider *triangleA = (PolygonCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
SphereCollider *sphereB = (SphereCollider *)convexB;
result = TriangleSphere(
triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal,
sphereB->Center, sphereB->Radius, aFromB);
break;
case ColliderType.Capsule:
result = CapsuleTriangle((CapsuleCollider *)convexB, triangleA, Inverse(aFromB));
flip = true;
break;
case ColliderType.Box:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref triangleA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Box:
BoxCollider *boxA = (BoxCollider *)convexA;
switch (convexB->Type)
{
case ColliderType.Sphere:
result = BoxSphere(boxA, (SphereCollider *)convexB, aFromB);
break;
case ColliderType.Capsule:
case ColliderType.Box:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref boxA->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
break;
case ColliderType.Quad:
case ColliderType.Cylinder:
case ColliderType.Convex:
result = ConvexConvex(ref ((ConvexCollider *)convexA)->ConvexHull, ref ((ConvexCollider *)convexB)->ConvexHull, aFromB);
break;
default:
throw new NotImplementedException();
}
if (flip)
{
result.PositionOnAinA = Mul(aFromB, result.PositionOnBinA);
result.NormalInA = math.mul(aFromB.Rotation, -result.NormalInA);
}
return(result);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Create a response
MBoolResponse response = new MBoolResponse(true);
//Set default values
this.abort = false;
this.filterSceneObjects = true;
//Assign the simulation state
this.simulationState = simulationState;
//Assign the instruction
this.instruction = instruction;
//Reset the elapsed time
this.elapsedTime = TimeSpan.Zero;
///Parse the properties
bool requiredPropertiesSet = this.ParseProperties(instruction);
//Check if all required properties have been set
if (!requiredPropertiesSet)
{
response.LogData = new List <string>()
{
"Properties are not defined -> cannot start the MMU"
};
response.Successful = false;
return(response);
}
//Get the target transform
MTransform targetTransform = this.GetTarget();
if (targetTransform == null)
{
response.Successful = false;
response.LogData = new List <string>()
{
"Problem at fetching the target transform!"
};
return(response);
}
//Flag indicates wheather a predefined trajectory should be used
bool usePredefinedTrajectory = false;
//Execute instructions on main thread
this.ExecuteOnMainThread(() =>
{
//Set the channel data of the current simulation state
this.SkeletonAccess.SetChannelData(this.simulationState.Current);
//Get the root position and rotation
MVector3 rootPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
MQuaternion rootRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
//Extract the start position
Vector2 startPosition = new Vector2((float)rootPosition.X, (float)rootPosition.Z);
//Get the goal position
this.goalPosition = new Vector2((float)targetTransform.Position.X, (float)targetTransform.Position.Z);
this.lastGoalPosition = this.goalPosition;
//Fetch the trajectory if available
if (instruction.Properties.ContainsKey("Trajectory"))
{
try
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.ID == instruction.Properties["Trajectory"]).PathConstraint;
//Get the actual trajectory from the path constraint
List <Vector2> pointList = pathConstraint.GetVector2List();
//Estimate the distance between the start point and first point of trajectory
float distance = (startPosition - pointList[0]).magnitude;
//If distance to first point of trajectory is below threshold -> Directly connect the start point with the first point of the trajectory
if (distance < 0.5)
{
pointList.Insert(0, startPosition);
trajectory = new MotionTrajectory2D(pointList, this.Velocity);
}
else
{
//Compute a path to the trajectory start location
this.trajectory = this.ComputePath(startPosition, pointList[0], this.filterSceneObjects);
//Add the defined trajectory
this.trajectory.Append(pointList);
//Finally estimate the timestamps based on the velocity
this.trajectory.EstimateTimestamps(this.Velocity);
}
//Set flag that no more planning is required
usePredefinedTrajectory = true;
}
catch (Exception e)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "UnityLocomotionMMU: Cannot parse trajectory -> plan new one: " + e.Message + " " + e.StackTrace);
}
}
//Only plan the path if no predefined trajectory should be used
if (!usePredefinedTrajectory)
{
//Compute a path which considers the indivudal path goals and the constraints
this.trajectory = this.ComputePath(startPosition, this.goalPosition, filterSceneObjects);
}
if (this.trajectory == null || this.trajectory.Poses.Count == 0)
{
this.abort = true;
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"No path found in assign instruction. Aborting MMU.");
response.LogData = new List <string>()
{
"No path found"
};
response.Successful = false;
}
//Valid path found
if (this.trajectory != null && this.trajectory.Poses.Count > 0)
{
//Create the visualization data for the trajectory (drawing calls)
this.CreateTrajectoryVisualization();
//Set the speed of the animator
this.animator.speed = this.Velocity / 2.0f;
//Reset the goal direction
this.goalPosition = new Vector2();
//Update the animation
this.animator.SetFloat("Velocity", 0);
this.animator.SetFloat("Direction", 0.5f);
//Reset the current index
this.currentIndex = 0;
//Set the internal fields
this.transform.position = new Vector3((float)rootPosition.X, 0, (float)rootPosition.Z);
// In the new intermediate skeleton definition, x is pointing backwards, y up, z right.
this.transform.rotation = Quaternion.Euler(0, new Quaternion((float)rootRotation.X, (float)rootRotation.Y, (float)rootRotation.Z, (float)rootRotation.W).eulerAngles.y - 90.0f, 0);
//Get the discrete poses representing the trajectory
this.discretePoses = this.trajectory.SampleFrames(60);
//Set state to starting
this.state = WalkState.Starting;
//Set flag which indicates the start
this.firstFrame = true;
//Reset the foot trackers
this.leftFootAnimationTracker.Reset();
this.rightFootAnimationTracker.Reset();
}
});
return(response);
}
public unsafe void Execute()
{
// Color palette
var colorA = Unity.DebugDisplay.ColorIndex.Cyan;
var colorB = Unity.DebugDisplay.ColorIndex.Magenta;
var colorError = Unity.DebugDisplay.ColorIndex.Red;
var colorRange = Unity.DebugDisplay.ColorIndex.Yellow;
OutputStream.Begin(0);
for (int iJoint = 0; iJoint < Joints.Length; iJoint++)
{
Joint joint = Joints[iJoint];
if (!joint.BodyPair.IsValid)
{
continue;
}
RigidBody bodyA = Bodies[joint.BodyPair.BodyIndexA];
RigidBody bodyB = Bodies[joint.BodyPair.BodyIndexB];
MTransform worldFromA, worldFromB;
MTransform worldFromJointA, worldFromJointB;
{
worldFromA = new MTransform(bodyA.WorldFromBody);
worldFromB = new MTransform(bodyB.WorldFromBody);
worldFromJointA = Mul(worldFromA, joint.AFromJoint);
worldFromJointB = Mul(worldFromB, joint.BFromJoint);
}
float3 pivotA = worldFromJointA.Translation;
float3 pivotB = worldFromJointB.Translation;
for (var i = 0; i < joint.Constraints.Length; i++)
{
Constraint constraint = joint.Constraints[i];
switch (constraint.Type)
{
case ConstraintType.Linear:
float3 diff = pivotA - pivotB;
// Draw the feature on B and find the range for A
float3 rangeOrigin;
float3 rangeDirection;
float rangeDistance;
switch (constraint.Dimension)
{
case 0:
continue;
case 1:
float3 normal = worldFromJointB.Rotation[constraint.ConstrainedAxis1D];
OutputStream.Plane(pivotB, normal * k_Scale, colorB);
rangeDistance = math.dot(normal, diff);
rangeOrigin = pivotA - normal * rangeDistance;
rangeDirection = normal;
break;
case 2:
float3 direction = worldFromJointB.Rotation[constraint.FreeAxis2D];
OutputStream.Line(pivotB - direction * k_Scale, pivotB + direction * k_Scale, colorB);
float dot = math.dot(direction, diff);
rangeOrigin = pivotB + direction * dot;
rangeDirection = diff - direction * dot;
rangeDistance = math.length(rangeDirection);
rangeDirection = math.select(rangeDirection / rangeDistance, float3.zero, rangeDistance < 1e-5);
break;
case 3:
OutputStream.Point(pivotB, k_Scale, colorB);
rangeOrigin = pivotB;
rangeDistance = math.length(diff);
rangeDirection = math.select(diff / rangeDistance, float3.zero, rangeDistance < 1e-5);
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
// Draw the pivot on A
OutputStream.Point(pivotA, k_Scale, colorA);
// Draw error
float3 rangeA = rangeOrigin + rangeDistance * rangeDirection;
float3 rangeMin = rangeOrigin + constraint.Min * rangeDirection;
float3 rangeMax = rangeOrigin + constraint.Max * rangeDirection;
if (rangeDistance < constraint.Min)
{
OutputStream.Line(rangeA, rangeMin, colorError);
}
else if (rangeDistance > constraint.Max)
{
OutputStream.Line(rangeA, rangeMax, colorError);
}
if (math.length(rangeA - pivotA) > 1e-5f)
{
OutputStream.Line(rangeA, pivotA, colorError);
}
// Draw the range
if (constraint.Min != constraint.Max)
{
OutputStream.Line(rangeMin, rangeMax, colorRange);
}
break;
case ConstraintType.Angular:
switch (constraint.Dimension)
{
case 0:
continue;
case 1:
// Get the limited axis and perpendicular in joint space
int constrainedAxis = constraint.ConstrainedAxis1D;
float3 axisInWorld = worldFromJointA.Rotation[constrainedAxis];
float3 perpendicularInWorld = worldFromJointA.Rotation[(constrainedAxis + 1) % 3] * k_Scale;
// Draw the angle of A
OutputStream.Line(pivotA, pivotA + perpendicularInWorld, colorA);
// Calculate the relative angle
float angle;
{
float3x3 jointBFromA = math.mul(math.inverse(worldFromJointB.Rotation), worldFromJointA.Rotation);
angle = CalculateTwistAngle(new quaternion(jointBFromA), constrainedAxis);
}
// Draw the range in B
float3 axis = worldFromJointA.Rotation[constraint.ConstrainedAxis1D];
OutputStream.Arc(pivotB, axis, math.mul(quaternion.AxisAngle(axis, constraint.Min - angle), perpendicularInWorld), constraint.Max - constraint.Min, colorB);
break;
case 2:
// Get axes in world space
int axisIndex = constraint.FreeAxis2D;
float3 axisA = worldFromJointA.Rotation[axisIndex];
float3 axisB = worldFromJointB.Rotation[axisIndex];
// Draw the cones in B
if (constraint.Min == 0.0f)
{
OutputStream.Line(pivotB, pivotB + axisB * k_Scale, colorB);
}
else
{
OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Min, colorB);
}
if (constraint.Max != constraint.Min)
{
OutputStream.Cone(pivotB, axisB * k_Scale, constraint.Max, colorB);
}
// Draw the axis in A
OutputStream.Arrow(pivotA, axisA * k_Scale, colorA);
break;
case 3:
// TODO - no idea how to visualize this if the limits are nonzero :)
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
}
}
OutputStream.End();
}
/// <summary>
/// Updates the motion
/// </summary>
/// <param name="timespan"></param>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a simulation result
MSimulationResult result = new MSimulationResult()
{
Posture = simulationState.Current,
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Events = simulationState.Events
};
//Abort the whole MMU if required
if (abort)
{
result.Events.Add(new MSimulationEvent("No path found", MMIStandard.mmiConstants.MSimulationEvent_Abort, this.instruction.ID));
return(result);
}
List <MConstraint> constraints = result.Constraints;
//Get the current target transform
MTransform targetTransform = this.GetTarget();
if (targetTransform == null)
{
Debug.Log("No target defined -> error");
result.LogData = new List <string>()
{
"No target defined -> error"
};
return(result);
}
//Update the goal position
this.goalPosition = new Vector2((float)targetTransform.Position.X, (float)targetTransform.Position.Z);
//Set the current state
this.simulationState = simulationState;
TimeSpan timespan = TimeSpan.FromSeconds(time);
//Clear all events
this.events.Clear();
///Moreover the drawing calls
if (!firstFrame)
{
this.drawingCalls = null;
}
//Set started flag to false
if (firstFrame)
{
firstFrame = false;
}
//Execute instructions on main thread
this.ExecuteOnMainThread(() =>
{
try
{
if (this.state != WalkState.Idle)
{
//Update the tracker for tracking the velocity
this.leftFootAnimationTracker.UpdateStats(this.animator.GetBoneTransform(HumanBodyBones.LeftFoot), (float)time);
this.rightFootAnimationTracker.UpdateStats(this.animator.GetBoneTransform(HumanBodyBones.RightFoot), (float)time);
//Do the local steering
this.FollowPathRootTransform((float)time);
//Update the animator and sample new animation
this.animator.Update((float)time);
//Get the posture of the animation at the specified time
this.simulationState.Current = this.GetPosture();
//Increment the time
this.timespan = timespan;
this.elapsedTime += timespan;
}
result.Posture = this.GetRetargetedPosture();
result.Events = this.events;
result.DrawingCalls = this.drawingCalls;
//Set constraints relevant for the posture
//this.constraintManager.SetEndeffectorConstraint(MEndeffectorType.LeftFoot, this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.LeftAnkle), this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, MJointType.LeftAnkle));
//this.constraintManager.SetEndeffectorConstraint(MEndeffectorType.RightFoot, this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.RightAnkle), this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, MJointType.RightAnkle));
//this.constraintManager.SetEndeffectorConstraint(MEndeffectorType.LeftHand, result.Posture.GetGlobalPosition(MJointType.LeftWrist), result.Posture.GetGlobalRotation(MJointType.LeftWrist));
//this.constraintManager.SetEndeffectorConstraint(MEndeffectorType.RightHand, result.Posture.GetGlobalPosition(MJointType.RightWrist), result.Posture.GetGlobalRotation(MJointType.RightWrist));
}
catch (Exception e)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Problem within do-step of UnityLocomotionMMU: {e.Message} {e.StackTrace}");
}
});
return(result);
}
public void TransformNewHits(int oldNumHits, float oldFraction, MTransform transform, int rigidBodyIndex)
{
m_ClosestHit.Transform(transform, rigidBodyIndex);
CheckIsAcceptable(oldFraction);
}
/// <summary>
/// Method does the local steering
/// </summary>
/// <param name="time">The planning time for the present frame</param>
private void FollowPathRootTransform(float time)
{
//Use the root transform
Vector2 currentPosition = new Vector2(transform.position.x, transform.position.z);
Quaternion currentRotation = transform.rotation;
//Check if target position has changed
float dist = (this.lastGoalPosition - this.goalPosition).magnitude;
//Estimate the distance to the goal
float goalDistance = (currentPosition - this.goalPosition).magnitude;
bool replanPath = false;
//Check if the target object has changed -> Replanning required
if (dist > 0.1f)
{
this.goalPosition = new Vector2(this.goalPosition.x, this.goalPosition.y);
replanPath = true;
}
//Check if replanning is enforced by replanning time
else
{
replanPath = goalDistance > 0.5f && replanningTime > 0 && elapsedTime.TotalMilliseconds > 0 && (int)elapsedTime.TotalMilliseconds % this.replanningTime == 0;
}
//Optionally do reactive replanning
if (replanPath)
{
//Compute a new path
this.trajectory = this.ComputePath(new Vector2(transform.position.x, transform.position.z), this.goalPosition);
//Get the discrete poses from the path
this.discretePoses = this.trajectory.SampleFrames(60);
//Reset index and first frame flag
this.currentIndex = 0;
this.firstFrame = true;
//Create a visualization of the trajectory (drawing calls)
this.CreateTrajectoryVisualization();
}
//Get the next waypoint
Vector2 nextWaypoint = this.GetNextWaypoint(this.trajectory, currentPosition, this.TimeHorizon);
//Use the goal position directly if below threshold
if (goalDistance < 0.5)
{
nextWaypoint = new Vector2(this.goalPosition.x, this.goalPosition.y);
}
switch (this.state)
{
//Rotate towards the target before starting
case WalkState.Starting:
//Call the method of the dhm
if (this.OrientateTowards(nextWaypoint, this.timespan, 5))
{
//Change the state if finished
this.state = WalkState.Walking;
}
break;
//The main state during walking
case WalkState.Walking:
//Determine the next target position
Vector2 nextTarget = this.GetNextWaypoint(this.trajectory, currentPosition, this.TimeHorizon);
//Check if the goal distance is below a defined threhsold -> Set the next target to goal position
if (goalDistance < 0.5)
{
nextTarget = new Vector2(this.goalPosition.x, this.goalPosition.y);
}
//Determine the trajectory direction
Vector2 trajectoryDirection = (nextTarget - currentPosition);
//Estimate the max distance allowed
float travelDistance = time * this.Velocity;
//Compute the new position
Vector2 nextPosition = currentPosition + trajectoryDirection.normalized * travelDistance;
//Update the position and rotation according to the animation
this.transform.position = this.animator.rootPosition;
this.transform.rotation = this.animator.rootRotation;
//The current direction vector
Vector3 currentDirection = (this.transform.rotation * Vector3.forward);
currentDirection.y = 0;
//Estimate the current angle mismatch
float currentDeltaAngle = Vector3.Angle(currentDirection, new Vector3(trajectoryDirection.x, 0, trajectoryDirection.y));
//Rotate to direction
//Adjust the rotation
//Only adjust the rotation if a sufficient distance to the goal can be encountred
//In case the distance is too low -> Turning in place is used instead
if (Math.Abs(currentDeltaAngle) > 1e-5f && goalDistance > 0.3f)
{
float maxAngle = time * this.AngularVelocity;
float goalDelta = Math.Abs(currentDeltaAngle);
float delta = Math.Min(maxAngle, goalDelta);
Vector3 right = (this.transform.rotation * Vector3.right);
Vector3 left = (this.transform.rotation * Vector3.left);
//Determine the sign of the angle
float sign = -1;
if (Vector3.Angle(right, new Vector3(trajectoryDirection.x, 0, trajectoryDirection.y)) < Vector3.Angle(left, new Vector3(trajectoryDirection.x, 0, trajectoryDirection.y)))
{
sign = 1;
}
this.transform.rotation = Quaternion.Euler(0, sign * delta, 0) * this.transform.rotation;
}
//Update the animation -> Set the flots of the animator component to control the animation tree
this.animator.SetFloat("Velocity", ((nextPosition - currentPosition).magnitude / time));
this.animator.SetFloat("Direction", 0.5f);
//Check if target reached
if ((nextPosition - this.goalPosition).magnitude < this.goalAccuracy)
{
//Set velocity to zero
this.animator.SetFloat("Velocity", 0f);
//Check velocity threshold if defined
if (!useVelocityStoppingThreshold || (this.leftFootAnimationTracker.Velocity < velocityStoppingThreshold && this.rightFootAnimationTracker.Velocity < velocityStoppingThreshold))
{
//Go to finishing state and perform further reorientation
if (this.useTargetOrientation)
{
this.state = WalkState.Finishing;
}
//Finish and move to idle
else
{
this.state = WalkState.Idle;
this.events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
}
}
break;
//The finishing state which performs the final fine-grained rotation
case WalkState.Finishing:
//Get the ttarget transform
MTransform targetTransform = this.GetTarget();
MVector3 forward = targetTransform.Rotation.Multiply(new MVector3(0, 0, 1));
Vector2 forward2 = new Vector2((float)forward.X, (float)forward.Z).normalized;
bool validOrientation = false;
if (this.OrientateTowards(goalPosition + forward2 * 10f, this.timespan, 5))
{
validOrientation = true;
}
//Only finish if velocity is close to zero
if (validOrientation)
{
//Set to finished -> Reset state machine
this.state = WalkState.Idle;
//Add finished event
this.events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
break;
}
}
public static float3 Mul(MTransform a, float3 x)
{
return(math.mul(a.Rotation, x) + a.Translation);
}
public void ScaleSkeleton(MAvatarPosture globalTarget, Dictionary <MJointType, string> joint_map)
{
List <MJointType> spine = new List <MJointType>()
{
MJointType.S1L5Joint, MJointType.T12L1Joint, MJointType.T1T2Joint, MJointType.C4C5Joint, MJointType.HeadJoint
};
if (spine.Contains(this.GetMJoint().Type))
{
//float shoulder_height = getJointPosition(HumanBodyBones.LeftUpperArm, anim).y;
double shoulder_height = getJointPosition(globalTarget, joint_map[MJointType.HeadJoint]).Y;
double hip_height = getJointPosition(globalTarget, joint_map[MJointType.PelvisCentre]).Y;
double spine_length = shoulder_height - hip_height;
((RJoint)this.parentJoint).boneLength = (this.GetOffsetPositions()).Multiply(spine_length).Magnitude();
this.GetOffsetPositions().X = 0;
this.GetOffsetPositions().Y = 1.0;
this.GetOffsetPositions().Z = 0;
}
if (this.joint.Type == MJointType.Root)
{
//MVector3 rootPos = getJointPosition(globalTarget, joint_map[MJointType.Root]);
//this.SetOffsets(rootPos);
}
else if (this.joint.Type == MJointType.PelvisCentre)
{
MVector3 hips = getJointPosition(globalTarget, joint_map[MJointType.PelvisCentre]).Clone();
MVector3 shoulder = getJointPosition(globalTarget, joint_map[MJointType.LeftShoulder]);
MVector3 leftupleg = getJointPosition(globalTarget, joint_map[MJointType.LeftHip]);
// find the average z axis position of shoulders and hips to find initial position of hip
hips.Z = (shoulder.Z + leftupleg.Z) / 2;
// raise hip to the hight of the target avatars hip height.
hips.Y = leftupleg.Y;
MVector3 root = new MVector3(hips.X, 0, hips.Z);
((RJoint)this.parentJoint).SetOffsets(root);
hips.X = 0;
hips.Z = 0;
this.SetOffsets(hips);
}
else
{
double factor = 0.0d;
if (this.parentJoint.children.Count == 3 && this.parentJoint.children[0] != this)
{
// If there are two children (e.g. pelvis / upper spine), take the half distance between both children
factor = GetJointDistance(globalTarget, joint_map[this.parentJoint.children[1].GetMJoint().Type], joint_map[this.parentJoint.children[2].GetMJoint().Type]) / 2.0d;
}
else
{
// If there is one child, take the parent bone length to scale target position
factor = ((RJoint)this.parentJoint).boneLength;
}
// scale offset to scale the skeleton to match bone lengths.
this.SetOffsets(this.GetOffsetPositions().Multiply(factor));
// handle special cases for shoulders and hands
string jointID = this.GetMJoint().ID;
if (jointID.Contains("Shoulder"))
{
// in case of shoulders, they have to be raised in up direction.
double shoulder_height = getJointPosition(globalTarget, joint_map[this.GetMJoint().Type]).Y;
double current_Pos = this.GetShoulderHeight();
shoulder_height = shoulder_height - current_Pos;
this.GetOffsetPositions().Y = shoulder_height;
}
}
if (this.children.Count > 0 && this.children[0].GetMJoint().ID.Contains("Tip"))
{
// Finger Tips
MVector3 offsetVector = this.children[0].GetOffsetPositions();
this.boneLength = offsetVector.Magnitude();
}
else
{
// if this is not the last joint in a sequence, scale depending on distance to child human bone
if (joint_map.ContainsKey(this.children[0].GetMJoint().Type) && joint_map.ContainsKey(this.GetMJoint().Type))
{
this.boneLength = GetJointDistance(globalTarget, joint_map[this.GetMJoint().Type], joint_map[this.children[0].GetMJoint().Type]);
}
else
{
this.boneLength = 0.1;
}
}
// recurse
foreach (Joint child in this.children)
{
if (!child.GetMJoint().ID.Contains("Tip"))
{
((RJoint)(child)).ScaleSkeleton(globalTarget, joint_map);
}
}
if (this.GetMJoint().ID.Contains("Wrist"))
{
MVector3 wristPos = this.GetGlobalPosition();//getJointPosition(globalTarget, joint_map[this.GetMJoint().Type]);
MTransform Twrist = new MTransform("tmp", this.GetGlobalPosition(), this.GetGlobalRotation());
Joint middle = this.children[0];
Joint index = this.children[1];
Joint ring = this.children[2];
Joint little = this.children[3];
Joint thumb = this.children[4];
double middle_shift = 0.0;
if (joint_map.ContainsKey(middle.GetMJoint().Type))
{
MVector3 MiddlePos = ((RJoint)middle).GetFingerPosition(globalTarget, joint_map, 0);
middle_shift = MiddlePos.Z;
MiddlePos.Z = 0;
((RJoint)this.children[0]).SetOffsets(MiddlePos);
}
else
{
((RJoint)this.children[0]).SetOffsets(new MVector3(0, 0.1, 0));
}
if (joint_map.ContainsKey(index.GetMJoint().Type))
{
MVector3 IndexPos = ((RJoint)index).GetFingerPosition(globalTarget, joint_map, middle_shift);
((RJoint)this.children[1]).SetOffsets(IndexPos);
}
else
{
((RJoint)this.children[1]).SetOffsets(new MVector3(0, 0.1, 0.02));
}
if (joint_map.ContainsKey(ring.GetMJoint().Type))
{
MVector3 RingPos = ((RJoint)ring).GetFingerPosition(globalTarget, joint_map, middle_shift);
((RJoint)this.children[2]).SetOffsets(RingPos);
}
else
{
((RJoint)this.children[2]).SetOffsets(new MVector3(0, 0.1, -0.02));
}
if (joint_map.ContainsKey(little.GetMJoint().Type))
{
MVector3 LittlePos = ((RJoint)little).GetFingerPosition(globalTarget, joint_map, middle_shift);
((RJoint)this.children[3]).SetOffsets(LittlePos);
}
else
{
((RJoint)this.children[3]).SetOffsets(new MVector3(0, 0.1, -0.04));
}
if (joint_map.ContainsKey(thumb.GetMJoint().Type))
{
MVector3 ThumbPos = ((RJoint)thumb).GetFingerPosition(globalTarget, joint_map, middle_shift);
((RJoint)this.children[4]).SetOffsets(ThumbPos);
}
else
{
((RJoint)this.children[4]).SetOffsets(new MVector3(0, 0.02, 0.02));
}
}
}