private IntPtr allocateConvex(MeshCollider meshCollider, Matrix4x4 localToParentRelative)
{
if (meshCollider.sharedMesh == null)
{
throw new NotImplementedException("MeshCollider missing sharedMesh.");
}
if (meshCollider.convex == false)
{
throw new NotImplementedException("MeshCollider must be convex.");
}
Mesh mesh = meshCollider.sharedMesh;
INTERACTION_CONVEX_POLYHEDRON_DESCRIPTION meshDesc = new INTERACTION_CONVEX_POLYHEDRON_DESCRIPTION();
meshDesc.shape.type = ShapeType.Convex;
meshDesc.radius = 0.0f;
meshDesc.nVerticies = (uint)mesh.vertexCount;
meshDesc.pVertices = new LEAP_VECTOR[mesh.vertexCount];
for (int i = 0; i < mesh.vertexCount; i++)
{
LEAP_VECTOR v = localToParentRelative.MultiplyPoint(mesh.vertices[i]).ToCVector();
meshDesc.pVertices[i] = v;
}
IntPtr meshPtr = StructAllocator.AllocateStruct(ref meshDesc);
return(meshPtr);
}
public static ReturnStatus GetScale(ref LEAP_IE_KABSCH kabsch,
out LEAP_VECTOR translation)
{
var rs = LeapIEKabschGetScale(ref kabsch, out translation);
Logger.HandleReturnStatus("GetScale", LogLevel.AllCalls, rs);
return(rs);
}
public static ReturnStatus SolveWithPlanar(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR planeNormal)
{
var rs = LeapIEKabschSolveWithPlanar(ref kabsch, ref planeNormal);
Logger.HandleReturnStatus("SolveWithPlanar", LogLevel.AllCalls, rs);
return(rs);
}
public static ReturnStatus SolveWithPivot(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR pivot)
{
var rs = LeapIEKabschSolveWithPivot(ref kabsch, ref pivot);
Logger.HandleReturnStatus("SolveWithPivot", LogLevel.AllCalls, rs);
return(rs);
}
public static LEAP_VECTOR ToCVector(this Vector3 vector)
{
LEAP_VECTOR cVector = new LEAP_VECTOR();
cVector.x = vector.x;
cVector.y = vector.y;
cVector.z = vector.z;
return(cVector);
}
public static ReturnStatus AddNormal(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR normal1,
ref LEAP_VECTOR normal2,
float weight)
{
var rs = LeapIEKabschAddNormal(ref kabsch, ref normal1, ref normal2, weight);
Logger.HandleReturnStatus("AddNormal", LogLevel.AllCalls, rs);
return(rs);
}
public static ReturnStatus AddPoint(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR point1,
ref LEAP_VECTOR point2,
float weight)
{
var rs = LeapIEKabschAddPoint(ref kabsch, ref point1, ref point2, weight);
Logger.HandleReturnStatus("AddPoint", LogLevel.AllCalls, rs);
return(rs);
}
protected void performSolve()
{
switch (_solveMethod)
{
case SolveMethod.SixDegreeSolve:
KabschC.Solve(ref _kabsch);
break;
case SolveMethod.PivotAroundOrigin:
LEAP_VECTOR v = new LEAP_VECTOR();
v.x = v.y = v.z = 0;
KabschC.SolveWithPivot(ref _kabsch, ref v);
break;
}
}
public override void GetHoldingPose(ReadonlyList <Hand> hands, out Vector3 newPosition, out Quaternion newRotation)
{
KabschC.Reset(ref _kabsch);
Vector3 bodyPosition = _obj.warper.RigidbodyPosition;
Quaternion bodyRotation = _obj.warper.RigidbodyRotation;
Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
for (int h = 0; h < hands.Count; h++)
{
Hand hand = hands[h];
var collection = _handIdToPoints[hand.Id];
for (int f = 0; f < NUM_FINGERS; f++)
{
Finger finger = hand.Fingers[f];
Finger.FingerType fingerType = finger.Type;
for (int j = 0; j < NUM_BONES; j++)
{
Bone.BoneType boneType = (Bone.BoneType)j;
Bone bone = finger.Bone(boneType);
Vector3 localPos = collection.GetLocalPosition(fingerType, boneType);
Vector3 bonePos = bone.NextJoint.ToVector3();
//Do the solve such that the objects positions are matched to the new bone positions
LEAP_VECTOR point1 = (it.MultiplyPoint3x4(localPos) - bodyPosition).ToCVector();
LEAP_VECTOR point2 = (bonePos - bodyPosition).ToCVector();
KabschC.AddPoint(ref _kabsch, ref point1, ref point2, 1.0f);
}
}
}
performSolve();
LEAP_VECTOR leapTranslation;
LEAP_QUATERNION leapRotation;
KabschC.GetTranslation(ref _kabsch, out leapTranslation);
KabschC.GetRotation(ref _kabsch, out leapRotation);
newPosition = bodyPosition + leapTranslation.ToVector3();
newRotation = leapRotation.ToQuaternion() * bodyRotation;
}
public HeadPoseEventArgs(LEAP_VECTOR head_position, LEAP_QUATERNION head_orientation) : base(LeapEvent.EVENT_POINT_MAPPING_CHANGE)
{
this.headPosition = head_position;
this.headOrientation = head_orientation;
}
public static Vector3 ToVector3(this LEAP_VECTOR vector)
{
return(new Vector3(vector.x, vector.y, vector.z));
}
private static extern ReturnStatus LeapIEKabschAddNormal(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR normal1,
ref LEAP_VECTOR normal2,
float weight);
private static extern ReturnStatus LeapIEKabschAddPoint(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR point1,
ref LEAP_VECTOR point2,
float weight);
private static extern ReturnStatus LeapIEKabschGetScale(ref LEAP_IE_KABSCH kabsch,
out LEAP_VECTOR translation);
private static extern ReturnStatus LeapIEKabschSolveWithPlanar(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR planeNormal);
private static extern ReturnStatus LeapIEKabschSolveWithPivot(ref LEAP_IE_KABSCH kabsch,
ref LEAP_VECTOR pivot);
