// Returns the rotation Quaternion of the given bone in relation to the controller.
public Quaternion GetBoneRotation(int bone_type)
{
Bone.BoneType type = (Bone.BoneType)bone_type;
Quaternion local_rotation = finger_.Bone(type).Basis.Rotation();
return(controller_.transform.rotation * local_rotation);
}
// 
FingerBone GetBone(Finger finger, Bone.BoneType boneType)
{
Bone bone = finger.Bone(boneType);
ScaledPoint centerPt = LeapCalibrator.ToScaledPoint(bone.Center);
return(new FingerBone(centerPt, bone.Length * centerPt.Scale, new DndCore.Vector(bone.Direction.x, bone.Direction.y)));
}
public bool GetBoneFromDB(int id)
{
mscon.Open();//
string msg = "select * from bone where bone_id=" + id;
MySqlCommand mscmd = new MySqlCommand(msg, mscon);
MySqlDataReader reader = mscmd.ExecuteReader();
int counter = 0;
if (reader.Read())
{
if (counter > 0)
{
Console.WriteLine("answer not unique");
bone = new Bone();
return(false);
}
Bone.BoneType bt_buffer = (Bone.BoneType)reader.GetInt32(15);
Bone buffer = new Bone(
new Vector(reader.GetFloat(1), reader.GetFloat(2), reader.GetFloat(3)),
new Vector(reader.GetFloat(4), reader.GetFloat(5), reader.GetFloat(6)),
new Vector(reader.GetFloat(7), reader.GetFloat(8), reader.GetFloat(9)),
new Vector(reader.GetFloat(10), reader.GetFloat(11), reader.GetFloat(12)),
reader.GetFloat(13),
reader.GetFloat(14),
bt_buffer,
new LeapQuaternion(reader.GetFloat(16), reader.GetFloat(17), reader.GetFloat(18), reader.GetFloat(19))
);
counter++;
bone = buffer;
}
mscon.Close();//
return(true);
}
public static string BoneTypeToString(Bone.BoneType boneType)
{
string boneName = boneType.ToString().Split('_').LastOrDefault().ToLower();
boneName = Char.ToUpper(boneName[0]) + boneName.Substring(1);
return(boneName);
}
/**
* Constructs an invalid Bone object.
*
* \include Bone_invalid.txt
*
* Get valid Bone objects from a Finger object.
*
* @since 2.0
*/
public Bone()
{
_prevJoint = Vector.Zero;
_nextJoint = Vector.Zero;
_basis = Matrix.Identity;
_center = Vector.Zero;
_direction = Vector.Zero;
_type = BoneType.TYPE_METACARPAL; //There is no invalid BoneType
}
public void FingersHaveFourBones([ValueSource(typeof(FrameValidator), "_fingers")] Finger.FingerType fingerType,
[ValueSource(typeof(FrameValidator), "_bones")] Bone.BoneType boneType)
{
foreach (Hand hand in _frame.Hands)
{
Bone bone = getBone(hand, fingerType, boneType);
Assert.That(bone, Is.Not.Null);
}
}
/**
* Constructs an invalid Bone object.
*
* \include Bone_invalid.txt
*
* Get valid Bone objects from a Finger object.
*
* @since 2.0
*/
public Bone()
{
_prevJoint = Vector.Zero;
_nextJoint = Vector.Zero;
_basis = Matrix.Identity;
_center = Vector.Zero;
_direction = Vector.Zero;
_type = BoneType.TYPE_METACARPAL; //There is no invalid BoneType
}
public Bone makeBone(ref LEAP_BONE bone, Bone.BoneType type)
{
Vector prevJoint = new Vector(bone.prev_joint.x, bone.prev_joint.y, bone.prev_joint.z);
Vector nextJoint = new Vector(bone.next_joint.x, bone.next_joint.y, bone.next_joint.z);
Vector center = (nextJoint + prevJoint) * .5f;
float length = (nextJoint - prevJoint).Magnitude;
Vector direction = (nextJoint - prevJoint) / length;
LeapQuaternion rotation = new LeapQuaternion(bone.rotation);
return(new Bone(prevJoint, nextJoint, center, direction, length, bone.width, type, rotation));
}
public override void GetHoldingPose(ReadonlyList <Hand> hands, out Vector3 newPosition, out Quaternion newRotation)
{
points.Clear(); refPoints.Clear();
Vector3 bodyPosition = _obj.warper.RigidbodyPosition;
Quaternion bodyRotation = _obj.warper.RigidbodyRotation;
Matrix4x4 it = Matrix4x4.TRS(bodyPosition, bodyRotation, Vector3.one);
handCentroid = Vector3.zero; objectCentroid = Vector3.zero; boneCount = 0f;
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
Vector3 point1 = (it.MultiplyPoint3x4(localPos) - bodyPosition);
Vector3 point2 = (bonePos - bodyPosition);
switch (_solveMethod)
{
case SolveMethod.SixDegreeSolve:
//Set the relevant points in each array
points.Add(point1); refPoints.Add(point2);
break;
case SolveMethod.PivotAroundOrigin:
//Calculate the Centroids of the object and hand(s) points
objectCentroid += point1;
handCentroid += point2;
boneCount += 1f;
break;
}
}
}
}
Matrix4x4 KabschTransform = performSolve(bodyPosition);
newPosition = bodyPosition + KabschTransform.GetVector3();
newRotation = KabschTransform.GetQuaternion() * bodyRotation;
}
public void CenterIsBetweenJoints([ValueSource(typeof(FrameValidator), "_fingers")] Finger.FingerType fingerType,
[ValueSource(typeof(FrameValidator), "_bones")] Bone.BoneType boneType)
{
foreach (Hand hand in _frame.Hands)
{
Bone bone = getBone(hand, fingerType, boneType);
Vector jointAverage = (bone.NextJoint + bone.PrevJoint) * 0.5f;
assertVectorsEqual(jointAverage, bone.Center);
}
}
public SerializableArm(Arm b)
{
prevJoint = new SerializableVector(b.PrevJoint);
nextJoint = new SerializableVector(b.NextJoint);
center = new SerializableVector(b.Center);
direction = new SerializableVector(b.Direction);
length = b.Length;
width = b.Width;
type = b.Type;
rotation = new SerializableLeapQuaternion(b.Rotation);
}
public void BoneLength([ValueSource(typeof(FrameValidator), "_fingers")] Finger.FingerType fingerType,
[ValueSource(typeof(FrameValidator), "_bones")] Bone.BoneType boneType)
{
foreach (Hand hand in _frame.Hands)
{
Bone bone = getBone(hand, fingerType, boneType);
float apparentLength = bone.NextJoint.DistanceTo(bone.PrevJoint);
float actualLength = bone.Length;
Assert.That(actualLength, Is.EqualTo(apparentLength).Within(TOLERANCE));
}
}
private static Bone makeBone(Bone.BoneType name, Vector proximalPosition, float length, float width, Vector direction, Vector up, bool isLeft)
{
LeapQuaternion rotation = UnityEngine.Quaternion.LookRotation(-direction.ToVector3(), up.ToVector3()).ToLeapQuaternion();
return(new Bone(
proximalPosition,
proximalPosition + direction * length,
Vector.Lerp(proximalPosition, proximalPosition + direction * length, .5f),
direction,
length,
width,
name,
rotation));
}
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 void DirectionMatchesJoints([ValueSource(typeof(FrameValidator), "_fingers")] Finger.FingerType fingerType,
[ValueSource(typeof(FrameValidator), "_bones")] Bone.BoneType boneType)
{
foreach (Hand hand in _frame.Hands)
{
Bone bone = getBone(hand, fingerType, boneType);
//If the joints are at the same position this test is meaningless
if (bone.NextJoint.DistanceTo(bone.PrevJoint) < TOLERANCE)
{
continue;
}
Vector jointDirection = (bone.NextJoint - bone.PrevJoint).Normalized;
assertVectorsEqual(jointDirection, bone.Direction);
}
}
static Bone MakeBone(Bone.BoneType name, Vector proximalPosition, float length, float width, Vector direction, Vector up)
{
Matrix basis = new Matrix();
basis.zBasis = direction;
basis.yBasis = up;
basis.xBasis = direction.Cross(up).Normalized;
return(new Bone(
proximalPosition,
proximalPosition + direction * length,
Vector.Lerp(proximalPosition, proximalPosition + direction * length, .5f),
direction,
length,
width,
name,
basis));
}
private void SetBone(Bone bone, Transform prev, Transform next, Bone.BoneType type,
float fingerWidth)
{
Vector3 up, forward, right;
GetBasis(prev, out right, out forward, out up);
float boneDist = Vector3.Distance(prev.position, next.position);
Vector3 boneCenter = (prev.position + next.position) * 0.5f;
bone.PrevJoint = prev.position.ToVector();
bone.NextJoint = next.position.ToVector();
bone.Center = boneCenter.ToVector();
bone.Direction = forward.ToVector();
bone.Length = boneDist;
bone.Width = fingerWidth;
bone.Type = type;
bone.Rotation = Quaternion.LookRotation(forward, up).ToLeapQuaternion();
}
protected Bone getBone(Hand hand, Finger.FingerType fingerType, Bone.BoneType boneType)
{
if (boneType < 0 || (int)boneType >= 4)
{
return(null);
}
foreach (Finger finger in hand.Fingers)
{
if (finger.Type != fingerType)
{
continue;
}
return(finger.Bone(boneType));
}
return(null);
}
public Arm(Vector prevJoint,
Vector nextJoint,
Vector center,
Vector direction,
float length,
float width,
Bone.BoneType type,
Matrix basis
) : base(prevJoint,
nextJoint,
center,
direction,
length,
width,
type,
basis)
{
}
/**
* Constructs a new Bone object.
*
* @param prevJoint The proximal end of the bone (closest to the body)
* @param nextJoint The distal end of the bone (furthest from the body)
* @param center The midpoint of the bone
* @param direction The unit direction vector pointing from prevJoint to nextJoint.
* @param length The estimated length of the bone.
* @param width The estimated average width of the bone.
* @param type The type of finger bone.
* @param basis The matrix representing the orientation of the bone.
* @since 3.0
*/
public Bone(Vector prevJoint,
Vector nextJoint,
Vector center,
Vector direction,
float length,
float width,
Bone.BoneType type,
LeapQuaternion rotation)
{
PrevJoint = prevJoint;
NextJoint = nextJoint;
Center = center;
Direction = direction;
Rotation = rotation;
Length = length;
Width = width;
Type = type;
}
/// <summary>
/// Fills the Bone object with the provided bone data.
/// </summary>
public static void Fill(this Bone toFill,
Vector prevJoint,
Vector nextJoint,
Vector center,
Vector direction,
float length,
float width,
Bone.BoneType type,
LeapQuaternion rotation)
{
toFill.PrevJoint = prevJoint;
toFill.NextJoint = nextJoint;
toFill.Center = center;
toFill.Direction = direction;
toFill.Length = length;
toFill.Width = width;
toFill.Type = type;
toFill.Rotation = rotation;
}
/**
* Constructs a new Bone object.
*
* @param prevJoint The proximal end of the bone (closest to the body)
* @param nextJoint The distal end of the bone (furthest from the body)
* @param center The midpoint of the bone
* @param direction The unit direction vector pointing from prevJoint to nextJoint.
* @param length The estimated length of the bone.
* @param width The estimated average width of the bone.
* @param type The type of finger bone.
* @param basis The matrix representing the orientation of the bone.
* @since 3.0
*/
public Bone(Vector prevJoint,
Vector nextJoint,
Vector center,
Vector direction,
float length,
float width,
Bone.BoneType type,
Matrix basis
)
{
PrevJoint = prevJoint;
NextJoint = nextJoint;
Center = center;
Direction = direction;
Basis = basis;
Length = length;
Width = width;
Type = type;
}
/**
* Copies the data from an internal bone definition into a bone.
*
* @param leapBone The internal bone definition to be copied into this bone.
* @param type The bone type of this bone.
*/
public static Bone CopyFrom(this Bone bone, LEAP_BONE leapBone, Bone.BoneType type) {
bone.Type = type;
bone.PrevJoint = leapBone.prev_joint.ToLeapVector();
bone.NextJoint = leapBone.next_joint.ToLeapVector();
bone.Direction = (bone.NextJoint - bone.PrevJoint);
bone.Length = bone.Direction.Magnitude;
if (bone.Length < float.Epsilon) {
bone.Direction = Vector.Zero;
} else {
bone.Direction /= bone.Length;
}
bone.Center = (bone.PrevJoint + bone.NextJoint) / 2.0f;
bone.Rotation = leapBone.rotation.ToLeapQuaternion();
bone.Width = leapBone.width;
return bone;
}
public Bone makeBone(ref LEAP_BONE bone, Bone.BoneType type)
{
Vector prevJoint = new Vector(bone.prev_joint.x, bone.prev_joint.y, bone.prev_joint.z);
Vector nextJoint = new Vector(bone.next_joint.x, bone.next_joint.y, bone.next_joint.z);
Vector center = (nextJoint + prevJoint) * .5f;
float length = (nextJoint - prevJoint).Magnitude;
Vector direction = (nextJoint - prevJoint) / length;
Matrix basis = new Matrix(bone.basis.x_basis.x,
bone.basis.x_basis.y,
bone.basis.x_basis.z,
bone.basis.y_basis.x,
bone.basis.y_basis.y,
bone.basis.y_basis.z,
bone.basis.z_basis.x,
bone.basis.z_basis.y,
bone.basis.z_basis.z);
return(new Bone(prevJoint, nextJoint, center, direction, length, bone.width, type, basis));
}
public Bone(Vector prevJoint,
Vector nextJoint,
Vector center,
Vector direction,
float length,
float width,
Bone.BoneType type,
Matrix basis
)
{
_prevJoint = prevJoint;
_nextJoint = nextJoint;
_center = center;
_direction = direction;
_basis = basis;
_length = length;
_width = width;
_type = type;
_isValid = true;
}
public Bone Bone(Bone.BoneType type)
{
switch (type)
{
case GR.StructV2.Bone.BoneType.TYPE_DISTAL:
return(B_Distal);
case GR.StructV2.Bone.BoneType.TYPE_INTERMEDIATE:
return(B_Intermediate);
case GR.StructV2.Bone.BoneType.TYPE_METACARPAL:
return(B_Metacarpal);
case GR.StructV2.Bone.BoneType.TYPE_PROXIMAL:
return(B_Proximal);
default:
throw new Exception("Unknow bone");
}
}
public void JointsMatch([ValueSource(typeof(FrameValidator), "_fingers")] Finger.FingerType fingerType,
[ValueSource(typeof(FrameValidator), "_bones")] Bone.BoneType boneType)
{
foreach (Hand hand in _frame.Hands)
{
Bone prevBone = getBone(hand, fingerType, boneType - 1);
Bone bone = getBone(hand, fingerType, boneType);
Bone nextBone = getBone(hand, fingerType, boneType + 1);
if (prevBone != null)
{
assertVectorsEqual(prevBone.NextJoint, bone.PrevJoint);
}
if (nextBone != null)
{
assertVectorsEqual(nextBone.PrevJoint, bone.NextJoint);
}
}
}
public override void AddHand(Hand hand)
{
var newCollection = HandPointCollection.Create(_obj.warper);
_handIdToPoints[hand.Id] = newCollection;
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 bonePos = bone.NextJoint.ToVector3();
//Global position of the point is just the position of the joint itself
newCollection.SetGlobalPosition(bonePos, fingerType, boneType);
}
}
}
JointData GetJointData(Finger finger, Bone.BoneType bonetype, int pn)
{
Quaternion rot;
Vector position;
Vector rotation;
if (pn == 0)
{
position = finger.Bone(bonetype).PrevJoint;
}
else
{
position = finger.Bone(bonetype).NextJoint;
}
rot = finger.Bone(bonetype).Basis.CalculateRotation();
Vector tmp = new Vector(rot.eulerAngles.x, rot.eulerAngles.y, rot.eulerAngles.z);
rotation = tmp;
JointData joint_data = new JointData(position, rotation);
return(joint_data);
}
/**
* The bone at a given bone index on this finger.
*
* \include Bone_iteration.txt
*
* @param boneIx An index value from the Bone::Type enumeration identifying the
* bone of interest.
* @returns The Bone that has the specified bone type.
* @since 2.0
*/
public Bone Bone(Bone.BoneType boneIx)
{
return(_bones[(int)boneIx]);
}
private SerializedProperty FingerOffsetPropertyFromLeapTypes(Finger.FingerType fingerType, Bone.BoneType boneType)
{
return(boundHand.FindPropertyRelative("fingers").GetArrayElementAtIndex((int)fingerType).FindPropertyRelative("boundBones").GetArrayElementAtIndex((int)boneType).FindPropertyRelative("offset"));
}
public Bone(Vector prevJoint,
Vector nextJoint,
Vector center,
Vector direction,
float length,
float width,
Bone.BoneType type,
Matrix basis
)
{
_prevJoint = prevJoint;
_nextJoint = nextJoint;
_center = center;
_direction = direction;
_basis = basis;
_length = length;
_width = width;
_type = type;
_isValid = true;
}
