private void SetupWrist()
{
if (this.glove != null && this.glove.gloveData.dataLoaded)
{
string ID = this.gloveData.deviceID;
if (this.gloveData.firmwareVersion <= 2.19f)
{
if (ID.Contains("120206"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quat.FromEuler(Mathf.PI / 2.0f, 0, Mathf.PI)); //correction for glove 1
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
else if (ID.Contains("120101"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quat.FromEuler(Mathf.PI, 0, 0)); //correction for glove 1
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
else if (ID.Contains("120203"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quat.FromEuler(0, 0, Mathf.PI / 2.0f)); //correction?
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
else if (ID.Contains("120307") || ID.Contains("120304") || ID.Contains("120310") || ID.Contains("120309") || ID.Contains("120311") || ID.Contains("120312"))
{
this.glove.gloveData.wrist.SetHardwareOrientation(Quat.FromEuler(0, 0, Mathf.PI)); //correction for glove 7 & 4?
SenseGlove_Debugger.Log("Firmware Version v2.19 or earlier. Adding Hardware Compensation");
}
}
}
}
public void EulerTest()
{
Quat qOurs = Quat.FromEuler(12 * Mathf.Deg2Rad, 97 * Mathf.Deg2Rad, 215 * Mathf.Deg2Rad);
Quaternion qUnity = Quaternion.Euler(12, 97, 215);
TestUtils.AssertQuat(qOurs, qUnity.w, qUnity.x, qUnity.y, qUnity.z);
}
void Update()
{
var vec = new Vec3(0, 0, 1).Normalize();
Debug.DrawRay(Vector3.zero, vec.ToVector3(), Color.red);
var vecTran = vec.Transform(Quat.FromEuler(0, MathUtilities.piHalf * .5f, 0));
Debug.DrawRay(Vector3.zero, vecTran.ToVector3(), Color.green);
}
public void QuatConjTest()
{
Quat originalOurs = Quat.FromEuler(12 * Mathf.Deg2Rad, 97 * Mathf.Deg2Rad, 215 * Mathf.Deg2Rad);
Quaternion originalUnity = UnityUtil.FromQuat(originalOurs);
Quat qOurs = originalOurs.Conjugate();
Quaternion qUnity = Quaternion.Inverse(originalUnity);
TestUtils.AssertQuat(qOurs, qUnity.w, qUnity.x, qUnity.y, qUnity.z);
}
public void VectorRotationTest()
{
Vec vOrigOurs = new Vec(1.2f, 3.4f, 5.6f);
Vector3 vOrigUnity = UnityUtil.FromVec(vOrigOurs);
Quat qOurs = Quat.FromEuler(12 * Mathf.Deg2Rad, 97 * Mathf.Deg2Rad, 215 * Mathf.Deg2Rad);
Quaternion qUnity = UnityUtil.FromQuat(qOurs);
Vec vOurs = qOurs * vOrigOurs;
Vector3 vUnity = qUnity * vOrigUnity;
TestUtils.AssertVec(vOurs, vUnity.x, vUnity.y, vUnity.z);
}
void Update()
{
var vec = new Vec3(0, 0, 1).Normalize();
Debug.DrawRay(Vector3.zero, vec.ToVector3(), Color.red);
var vecTran = vec.Transform(Quat.FromEuler(0, rot, 0));
Debug.DrawRay(Vector3.zero, vecTran.ToVector3(), Color.green);
rot += Time.deltaTime;
}
void FixedUpdate()
{
// get dumby current and last rotation values
var rotationLast = Quat.FromEuler(0, 0, 0);
var rotationCurrent = Quat.FromEuler(0, 0, 1);
// generate angular velocity
var angleVel = rotationLast.Inverse().Multiply(rotationCurrent).AngularVel();
// apply velocity to rigidbody
rigidbody.angularVelocity = angleVel.ToVector3();
}
private IValue YamlToValue(YamlNode yaml)
{
if (yaml.Tag == "!vec") // vector
{
float[] components = YamlToFloatArray(yaml, 3);
return(new Vec {
X = components[0], Y = components[1], Z = components[2]
});
}
else if (yaml.Tag == "!quat") // quaternion
{
float[] components = YamlToFloatArray(yaml, 4);
return(new Quat {
W = components[0], X = components[1], Y = components[2], Z = components[3]
});
}
else if (yaml.Tag == "!euler") // Euler angle is converted to a quaternion
{
float[] xyz = YamlToFloatArray(yaml, 3);
xyz = xyz.Select(deg => (float)(deg * Math.PI / 180.0)).ToArray(); // deg to rad
return(Quat.FromEuler(xyz[0], xyz[1], xyz[2]));
}
else if (yaml.Tag == "!load_file") // load local file and become a Blob handle
{
return(YamlHandleLoadFile(yaml));
}
else if (yaml.Tag == "!ref") // reference to a object
{
string name = YamlExpect <YamlScalarNode>(yaml).Value;
// TODO: Because current SceneLoader is one-pass, only reference to above-defined objects is supported.
return(new ObjectRef {
Id = namedObjects[name]
});
}
else if (yaml is YamlScalarNode scalar)
{
// Currently, type for YAML scalar is determined using TryParse.
// Therefore, it might be different from YAML spec https://yaml.org/spec/1.2/spec.html#id2805071 .
if (scalar.Style != YamlDotNet.Core.ScalarStyle.Plain)
{
return new Primitive <string> {
Value = scalar.Value
}
}
;
else if (int.TryParse(scalar.Value, out var intValue))
{
return new Primitive <int> {
Value = intValue
}
}
;
else if (float.TryParse(scalar.Value, out var floatValue))
{
return new Primitive <float> {
Value = floatValue
}
}
;
else if (TryParseBool(scalar.Value, out var boolValue))
{
return new Primitive <int> {
Value = boolValue ? 1 : 0
}
}
;
else
{
return new Primitive <string> {
Value = scalar.Value
}
};
}
else if (yaml is YamlSequenceNode seq)
{
return(new Sequence {
Elements = seq.Children.Select(YamlToValue).ToList()
});
}
else
{
ThrowError(yaml, "Invalid value");
return(null); // dummy (because this line is not recognized as unreachable)
}
}
