public SerializableLeapQuaternion(LeapQuaternion q)
{
x = q.x;
y = q.y;
z = q.z;
w = q.w;
}
// Update is called once per frame
void Update()
{
Leap.LeapQuaternion lq = hand.GetLeapHand().Rotation;
float z = new Quaternion(lq.x, lq.y, lq.z, lq.w).eulerAngles.z;
transform.rotation = Quaternion.Euler(0, 0, z);
}
public LeapTransform(Vector translation, LeapQuaternion rotation, Vector scale) :
this()
{
_scale = scale;
// these are non-trival setters.
this.translation = translation;
this.rotation = rotation; // Calls validateBasis
}
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));
}
private static LeapQuaternion angleAxis(float angle, Vector axis)
{
if (!axis.MagnitudeSquared.NearlyEquals(1.0f))
{
throw new ArgumentException("Axis must be a unit vector.");
}
float sineHalfAngle = Mathf.Sin(angle / 2.0f);
LeapQuaternion q = new LeapQuaternion(sineHalfAngle * axis.x,
sineHalfAngle * axis.y,
sineHalfAngle * axis.z,
Mathf.Cos(angle / 2.0f));
return(q.Normalized);
}
/// <summary>
/// Constructs a new Arm object.
/// @since 3.0
/// </summary>
public Arm(Vector elbow,
Vector wrist,
Vector center,
Vector direction,
float length,
float width,
LeapQuaternion rotation)
: base(elbow,
wrist,
center,
direction,
length,
width,
BoneType.TYPE_METACARPAL, //ignored for arms
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,
LeapQuaternion rotation)
{
PrevJoint = prevJoint;
NextJoint = nextJoint;
Center = center;
Direction = direction;
Rotation = rotation;
Length = length;
Width = width;
Type = type;
}
// This is only usable when the basis vectors have not been modified directly.
public LeapQuaternion TransformQuaternion(LeapQuaternion rhs)
{
if (_quaternionDirty)
{
throw new InvalidOperationException("Calling TransformQuaternion after Basis vectors have been modified.");
}
if (_flip)
{
// Mirror the axis of rotation accross the flip axis.
rhs.x *= _flipAxes.x;
rhs.y *= _flipAxes.y;
rhs.z *= _flipAxes.z;
}
LeapQuaternion t = _quaternion.Multiply(rhs);
return(t);
}
/// <summary>
/// Constructs a hand.
///
/// Generally, you should not create your own Hand objects. Such objects will not
/// have valid tracking data. Get valid Hand objects from a frame
/// received from the service.
/// @since 3.0
/// </summary>
public Hand(long frameID,
int id,
float confidence,
float grabStrength,
float grabAngle,
float pinchStrength,
float pinchDistance,
float palmWidth,
bool isLeft,
float timeVisible,
Arm arm,
List <Finger> fingers,
Vector palmPosition,
Vector stabilizedPalmPosition,
Vector palmVelocity,
Vector palmNormal,
LeapQuaternion palmOrientation,
Vector direction,
Vector wristPosition)
{
FrameId = frameID;
Id = id;
Confidence = confidence;
GrabStrength = grabStrength;
GrabAngle = grabAngle;
PinchStrength = pinchStrength;
PinchDistance = pinchDistance;
PalmWidth = palmWidth;
IsLeft = isLeft;
TimeVisible = timeVisible;
Arm = arm;
Fingers = fingers;
PalmPosition = palmPosition;
StabilizedPalmPosition = stabilizedPalmPosition;
PalmVelocity = palmVelocity;
PalmNormal = palmNormal;
Rotation = palmOrientation;
Direction = direction;
WristPosition = wristPosition;
}
/// <summary>
/// Returns a test Leap Hand object transformed by the leftHandTransform argument.
/// If the Leap hand is a right hand, the position and rotation of the Hand will be
/// mirrored along the X axis (so you can provide LeapTransform to construct both
/// left and right hands.
/// </summary>
public static Hand MakeTestHand(bool isLeft, LeapTransform leftHandTransform,
int frameId = 0, int handId = 0,
UnitType unitType = UnitType.LeapUnits)
{
// Apply the appropriate mirroring if this is a right hand.
if (!isLeft)
{
leftHandTransform.translation = new Vector(-leftHandTransform.translation.x,
leftHandTransform.translation.y, leftHandTransform.translation.z);
leftHandTransform.rotation = new LeapQuaternion(-leftHandTransform.rotation.x,
leftHandTransform.rotation.y,
leftHandTransform.rotation.z,
-leftHandTransform.rotation.w);
leftHandTransform.MirrorX();
}
// Leap space is oriented differently than Unity space, so correct for this here.
var hand = makeLeapSpaceTestHand(frameId, handId, isLeft)
.Transform(leftHandTransform);
var correctingQuaternion = Quaternion.Euler(90f, 0f, 180f);
var correctingLeapQuaternion = new LeapQuaternion(correctingQuaternion.x,
correctingQuaternion.y,
correctingQuaternion.z,
correctingQuaternion.w);
var transformedHand = hand.Transform(new LeapTransform(Vector.Zero,
correctingLeapQuaternion));
if (unitType == UnitType.UnityUnits)
{
transformedHand.TransformToUnityUnits();
}
return(transformedHand);
}
void Update()
{
Leap.Frame frame = _provider.CurrentFrame;
if (frame != null)
{
foreach (Leap.Hand hand in frame.Hands)
{
GameObject measured = hand.IsLeft ? lmeasured : rmeasured;
Leap.Bone tip = hand.Fingers[0].Bone(Leap.Bone.BoneType.TYPE_DISTAL);
// why is this necessary
Leap.Vector v = tip.NextJoint;
Vector3 uv = new Vector3(v.x, v.y, v.z);
Leap.LeapQuaternion q = tip.Rotation;
Quaternion uq = new Quaternion(q.x, q.y, q.z, q.w);
measured.transform.position = uv;
measured.transform.rotation = uq;
}
}
bool lTriggerDown = Input.GetAxis("LeftVRTriggerAxis") > 0.5;
bool rTriggerDown = Input.GetAxis("RightVRTriggerAxis") > 0.5;
bool triggerDown = lTriggerDown || rTriggerDown;
ltruth.SetActive(lTriggerDown);
rtruth.SetActive(rTriggerDown);
lmeasured.SetActive(lTriggerDown);
rmeasured.SetActive(rTriggerDown);
if (triggerDown)
{
if (!lastTriggerDown)
{
leapOrigin.transform.localPosition = Vector3.zero;
leapOrigin.transform.localRotation = Quaternion.identity;
truths = new List <Vector3>();
measurements = new List <Vector3>();
}
else
{
Transform cam = Camera.main.transform;
if (lmeasured.activeInHierarchy)
{
truths.Add(cam.InverseTransformPoint(ltruth.transform.position));
measurements.Add(cam.InverseTransformPoint(lmeasured.transform.position));
}
if (rmeasured.activeInHierarchy)
{
truths.Add(cam.InverseTransformPoint(rtruth.transform.position));
measurements.Add(cam.InverseTransformPoint(rmeasured.transform.position));
}
}
}
else
{
if (lastTriggerDown)
{
Vector3 truthCentroid = new Vector3(0, 0, 0);
Vector3 measurementCentroid = new Vector3(0, 0, 0);
int l = truths.Count;
foreach (Vector3 p in measurements)
{
measurementCentroid += p;
}
measurementCentroid /= truths.Count;
foreach (Vector3 p in truths)
{
truthCentroid += p;
}
truthCentroid /= truths.Count;
double[,] A = new double[3, l];
double[,] B = new double[3, l];
int i = 0;
foreach (Vector3 p in measurements)
{
Vector3 po = p - measurementCentroid;
A[0, i] = po.x;
A[1, i] = po.y;
A[2, i] = po.z;
i++;
}
i = 0;
foreach (Vector3 p in truths)
{
Vector3 po = p - truthCentroid;
B[0, i] = po.x;
B[1, i] = po.y;
B[2, i] = po.z;
i++;
}
double[,] BAT = new double[3, 3];
alglib.rmatrixgemm(3, 3, l, 1, B, 0, 0, 0, A, 0, 0, 1, 0, ref BAT, 0, 0);
double[] W = new double[3];
double[,] U = new double[3, 3];
double[,] VT = new double[3, 3];
alglib.rmatrixsvd(BAT, 3, 3, 2, 2, 2, out W, out U, out VT);
double[,] UVT = new double[3, 3];
alglib.rmatrixgemm(3, 3, 3, 1, U, 0, 0, 0, VT, 0, 0, 0, 0, ref UVT, 0, 0);
double w = Math.Sqrt(1 + UVT[0, 0] + UVT[1, 1] + UVT[2, 2]) / 2.0;
double iw4 = 1.0 / (w * 4);
Quaternion q = new Quaternion((float)((UVT[2, 1] - UVT[1, 2]) * iw4),
(float)((UVT[0, 2] - UVT[2, 0]) * iw4),
(float)((UVT[1, 0] - UVT[0, 1]) * iw4), (float)w);
Quaternion iq = Quaternion.Inverse(q);
Vector3 mtt = truthCentroid - measurementCentroid;
leapOrigin.transform.localPosition = truthCentroid - q * measurementCentroid;
leapOrigin.transform.localRotation = q;
StreamWriter writer = new StreamWriter("stored_calibration.txt");
writer.Write(String.Format("{0} {1} {2} {3} {4} {5} {6}",
leapOrigin.transform.localPosition.x,
leapOrigin.transform.localPosition.y,
leapOrigin.transform.localPosition.z,
leapOrigin.transform.localRotation.x,
leapOrigin.transform.localRotation.y,
leapOrigin.transform.localRotation.z,
leapOrigin.transform.localRotation.w
));
writer.Close();
truths = null;
measurements = null;
}
}
lastTriggerDown = triggerDown;
}
public LeapTransform(Vector translation, LeapQuaternion rotation) :
this(translation, rotation, Vector.Ones)
{
}
public static Vector4D ToVector4D(this Leap.LeapQuaternion V)
{
Vector4D tmp = new Vector4D((double)V.x, (double)V.y, (double)V.z, (double)V.w);
return(tmp);
}
private Quaternion FixQ(Leap.LeapQuaternion q)
{
return(new Quaternion(q.x, q.y, q.z, q.w));
}
public static Quaternion ToUnity(this Leap.LeapQuaternion q)
{
return(new Quaternion(q.x, q.y, q.z, q.w));
}
