void UpdateAngleUI(float[][] sensors)
{
float[][] angles = new float[sensors.Length][];
for (int f = 0; f < sensors.Length; f++)
{
angles[f] = new float[sensors[f].Length];
for (int j = 0; j < sensors[f].Length; j++)
{
float degr = Mathf.Round(sensors[f][j] * Mathf.Rad2Deg);
angles[f][j] = j < sensors[f].Length - 1 ? SG_Util.NormalizeAngle(degr)
: SG_Util.NormalizeAngle(degr, -60, 300);
}
}
if (angleCanvas != null)
{
for (int f = 0; f < angles.Length && f < angleBoxes.Length; f++)
{
for (int j = 0; j < angles[f].Length && j < angleBoxes[f].Length; j++)
{
angleBoxes[f][j].text = angles[f][j].ToString() + "°";
}
}
}
}
/// <summary> Calculate the angle of an absolute position relative to the hinge [Internal use] </summary>
/// <param name="absPosition"></param>
/// <returns></returns>
private float GetAngle(Vector3 absPosition)
{
//project the Object's Position on the plane that has our rotationAxis as a normal.
Vector3 proj = this.hingePoint.InverseTransformPoint(absPosition);
float res = 0;
if (this.hingeAxis == MovementAxis.X)
{
res = Mathf.Atan2(proj.z, proj.y);
}
else if (this.hingeAxis == MovementAxis.Y)
{
res = -Mathf.Atan2(proj.z, proj.x);
}
else if (this.hingeAxis == MovementAxis.Z)
{
res = Mathf.Atan2(proj.y, proj.x);
}
res = SenseGloveCs.Values.Degrees(res);
// SenseGlove_Debugger.Log("Position " + SG_Util.ToString(absPosition) + " ==> " + res);
res = SG_Util.NormalizeAngle(res);
return(res);
}
/// <summary> Normalize a set of (euler) angles to fall within a -180... 180 range. </summary>
/// <param name="angles"></param>
/// <returns></returns>
public static Vector3 NormalizeAngles(Vector3 angles)
{
return(new Vector3
(
SG_Util.NormalizeAngle(angles.x),
SG_Util.NormalizeAngle(angles.y),
SG_Util.NormalizeAngle(angles.z)
));
}
/// <summary> Contained in a separate method for child classes. </summary>
protected virtual void UpdateAngle()
{
Quaternion qDesiredAbs = this._grabReference.transform.rotation * this.rotOffset; //check the desired 3D angle
Quaternion qRotOriginal = this.hingePoint.transform.rotation; //save current rotation
this.hingePoint.rotation = qDesiredAbs; //perform rotation to calculate local angles
float angle = SG_Util.NormalizeAngle(this.hingePoint.localEulerAngles[angleIndex]) + this.anglOffset; //retrieve local angle
this.hingePoint.rotation = qRotOriginal; //return back now that we have the angle.
this.SetAngle(angle); //Apply the new rotation
}
//-------------------------------------------------------------------------------------------------------------------------
// Velocities / Transforms
#region Transforms
/// <summary> Calculate the angular velocity of a GameObject, using its current rotation and that of the previous frame. </summary>
/// <param name="currentRot"></param>
/// <param name="previousRot"></param>
/// <remarks>Placed here because it may be used by other scripts as well.</remarks>
/// <returns></returns>
public static Vector3 CalculateAngularVelocity(Quaternion currentRot, Quaternion previousRot, float deltaTime)
{
Quaternion dQ = currentRot * Quaternion.Inverse(previousRot);
Vector3 dE = dQ.eulerAngles;
Vector3 res = new Vector3
(
SG_Util.NormalizeAngle(dE.x),
SG_Util.NormalizeAngle(dE.y),
SG_Util.NormalizeAngle(dE.z)
);
return((res * Mathf.Deg2Rad) / deltaTime); //convert from deg to rad / sec
}
/// <summary> Validate the dial angle before applying it. </summary>
/// <param name="angle"></param>
/// <returns></returns>
public float ValidateAngle(float angle)
{
angle = SG_Util.NormalizeAngle(angle);
if (this.useLimits)
{
if (angle > this.maxAngle)
{
angle = this.maxAngle;
}
else if (angle < this.minAngle)
{
angle = this.minAngle;
}
}
return(angle);
}
/// <summary> Retrieve the local rotation angle of the hingePoint </summary>
/// <returns></returns>
public float GetHingeAngle()
{
Vector3 localAngles = this.hingePoint.localEulerAngles;
float angle = 0;
if (this.hingeAxis == MovementAxis.X)
{
angle = localAngles.x;
}
else if (this.hingeAxis == MovementAxis.Y)
{
angle = localAngles.y;
}
else if (this.hingeAxis == MovementAxis.Z)
{
angle = localAngles.z;
}
return(SG_Util.NormalizeAngle(angle));
}
