/// <summary> Setup the SG_BasicFeedback script components </summary>
public virtual void SetupSelf()
{
SG_Util.TryAddRB(this.gameObject, false, true);
cooldownTimer = 0;
SetTrackingTarget(this.trackingTarget, true); //updates offsets on start
this.lastPosition = this.transform.position;
}
/// <summary> Update this script's transform by applying a velocity to its rigidbody. </summary>
public void UpdateTransformPhysics()
{
if (this.trackedObject != null && this.handRB != null)
{
SG_Util.TransformRigidBody(ref this.handRB, this.TargetPosition, this.TargetRotation, physRotationSpeed);
}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
// Monobehaviour
//Load Resources before Start() function is called
protected virtual void Awake()
{
if (this.Hand == null)
{
this.Hand = SG_Util.CheckForTrackedHand(this.transform);
}
}
//----------------------------------------------------------------------------------------------
// Monobehaviour
protected override void Awake()
{
base.Awake();
SG_Util.CheckForHandInfo(this.transform, ref this.handModel);
if (this.handModel != null)
{
if (palmTouch != null && !palmTouch.HasTarget)
{
this.palmTouch.SetTrackingTarget(handModel.wristTransform, true);
}
Transform target;
if (thumbTouch != null && !thumbTouch.HasTarget && handModel.GetFingerTip(SG_HandSection.Thumb, out target))
{
this.thumbTouch.SetTrackingTarget(target, true);
}
if (indexTouch != null && !indexTouch.HasTarget && handModel.GetFingerTip(SG_HandSection.Index, out target))
{
this.indexTouch.SetTrackingTarget(target, true);
}
if (middleTouch != null && !middleTouch.HasTarget && handModel.GetFingerTip(SG_HandSection.Middle, out target))
{
this.middleTouch.SetTrackingTarget(target, true);
}
}
}
/// <summary> Updates all variables that can change during the simulation.</summary>
/// <param name="data"></param>
public void UpdateVariables(SenseGloveCs.GloveData data)
{
this.dataLoaded = data.dataLoaded;
this.deviceID = data.deviceID;
this.gloveValues = data.gloveValues;
this.imuValues = data.imuValues;
this.imuCalibration = data.imuCalibration;
this.numberOfSensors = data.numberOfSensors;
this.packetsPerSecond = data.samplesPerSec;
this.calibrationStep = data.currentCalStep;
this.totalCalibrationSteps = data.totalCalSteps;
this.absoluteCalibratedWrist = SG_Util.ToUnityQuaternion(data.wrist.QcalibratedAbs);
this.absoluteWrist = SG_Util.ToUnityQuaternion(data.wrist.QwristAbs);
this.relativeWrist = SG_Util.ToUnityQuaternion(data.wrist.Qrelative);
SenseGlove_Data.GetChainVariables(ref data.kinematics.gloveLinks, ref this.glovePositions,
ref this.gloveAngles, ref this.gloveRotations, ref this.gloveLengths);
SenseGlove_Data.GetChainVariables(ref data.kinematics.fingers, ref this.handPositions,
ref this.handAngles, ref this.handRotations, ref this.handLengths);
}
/// <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);
}
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> Update this object's transform by applying a velocity to the rigidbody </summary>
protected override void UpdatePosition()
{
if (trackingTarget != null)
{
if (this.physicsBody != null)
{
SG_Util.TransformRigidBody(ref physicsBody, this.TargetPosition, this.TargetRotation, rotationSpeed);
if ((this.transform.position - trackingTarget.position).magnitude > resetDistance)
{
resetTimer += this.updateTime == UpdateDuring.FixedUpdate ? Time.fixedDeltaTime : Time.deltaTime;
if (resetTimer >= resetTime)
{
base.UpdatePosition(); //snaps using simple method
resetTimer = 0;
}
}
else
{
resetTimer = 0;
}
}
else
{
base.UpdatePosition();
}
}
}
// Use this for initialization
void Start()
{
prevBtn.gameObject.SetActive(true);
nextBtn.gameObject.SetActive(true);
calibrateWristBtn.gameObject.SetActive(false);
layerObjects = new GameObject[(int)ShowingLayer.All][];
layerObjects[(int)ShowingLayer.FeedbackLayer] = this.feedbackObjects;
layerObjects[(int)ShowingLayer.RigidbodyLayer] = this.rigidBodyObjects;
layerObjects[(int)ShowingLayer.PhysicsLayer] = this.physicsObjects;
layerObjects[(int)ShowingLayer.GrabLayer] = this.grabLayerObjects;
SetAllObjects(false);
GoToStep(0);
//allow both hands to at least call Awake, then turn them off for now.
SG_Util.SetChildren(leftHand.transform, true);
SG_Util.SetChildren(leftHand.transform, false);
SG_Util.SetChildren(rightHand.transform, true);
SG_Util.SetChildren(rightHand.transform, false);
SG_Util.AppendButtonText(prevBtn, "\r\n(" + prevKey.ToString() + ")");
SG_Util.AppendButtonText(nextBtn, "\r\n(" + nextKey.ToString() + ")");
SG_Util.AppendButtonText(calibrateWristBtn, "\r\n(" + wristKey.ToString() + ")");
}
/// <summary> Assign scripts relevant to this script's functioning. </summary>
protected virtual void CheckForScripts()
{
SG_Util.CheckForHandInfo(this.transform, ref this.handModel);
if (this.Hand == null)
{
this.Hand = SG_Util.CheckForTrackedHand(this.transform);
}
}
/// <summary> Set a new tracking target for this script, which also calculates new offsets </summary>
/// <param name="newTarget"></param>
public virtual void SetTrackingTarget(Transform newTarget, bool calculateNewOffsets)
{
this.trackingTarget = newTarget;
if (trackingTarget != null && calculateNewOffsets)
{
SG_Util.CalculateOffsets(this.transform, trackingTarget, out this.positionOffset, out this.rotationOffset);
}
}
/// <summary> Checks for scripts that might be connected to this GameObject. Used in editor and during startup. </summary>
public override void CheckForScripts()
{
base.CheckForScripts();
if (this.gloveHardware == null)
{
this.gloveHardware = this.Hardware;
}
SG_Util.CheckForHandInfo(this.transform, ref this.handModel);
}
/// <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>
/// Convert an array of unity positions back into an array used by the DLL
/// </summary>
/// <param name="pos"></param>
/// <returns></returns>
public static SenseGloveCs.Kinematics.Vect3D[] ToPosition(Vector3[] pos)
{
SenseGloveCs.Kinematics.Vect3D[] res = new SenseGloveCs.Kinematics.Vect3D[pos.Length];
for (int f = 0; f < pos.Length; f++)
{
res[f] = SG_Util.ToPosition(pos[f]);
}
return(res);
}
public void ShowLayer(ShowingLayer layer)
{
if (this.showing != layer)
{
this.showing = layer;
if (activeHand != null)
{
SG_Util.SetChildren(activeHand.transform, false); //all layers are off
SetAllObjects(false);
if (layer >= ShowingLayer.None)
{
activeHand.handModel.gameObject.SetActive(true);
}
if (layer >= ShowingLayer.AnimationLayer)
{
activeHand.handAnimation.gameObject.SetActive(true);
}
activeHand.handModel.DebugEnabled = layer <ShowingLayer.FeedbackLayer && layer> ShowingLayer.None;
bool updateWrist = layer >= ShowingLayer.AnimationLayer;
if (updateWrist && !activeHand.handAnimation.updateWrist)
{
activeHand.handAnimation.updateWrist = true;
activeHand.handAnimation.CalibrateWrist();
activeHand.handAnimation.UpdateWrist(activeHand.hardware.GloveData);
}
if (!updateWrist)
{
activeHand.handAnimation.CalibrateWrist();
activeHand.handAnimation.UpdateWrist(activeHand.hardware.GloveData);
}
activeHand.handAnimation.updateWrist = updateWrist;
if (layer == ShowingLayer.FeedbackLayer || layer == ShowingLayer.All)
{
activeHand.feedbackScript.gameObject.SetActive(true);
}
if (layer == ShowingLayer.GrabLayer || layer == ShowingLayer.All)
{
activeHand.grabScript.gameObject.SetActive(true);
}
if (layer == ShowingLayer.RigidbodyLayer || layer == ShowingLayer.All)
{
activeHand.rigidBodyLayer.gameObject.SetActive(true);
}
if (layer == ShowingLayer.PhysicsLayer)
{
activeHand.physicsTrackingLayer.gameObject.SetActive(true);
}
}
SetLayerObjects((int)layer, true);
UpdateOverview(layer);
}
}
/// <summary> Check for Scripts relevant for this Animator </summary>
protected virtual void CheckForScripts()
{
if (this.Hand == null)
{
this.Hand = SG_Util.CheckForTrackedHand(this.transform);
}
if (this.senseGlove == null && this.Hand != null)
{
this.senseGlove = this.Hand.hardware;
}
}
/// <summary> Checks for scripts that might be connected to this GameObject. Used in editor and during startup. </summary>
public virtual void CheckForScripts()
{
if (connectedGlove == null)
{
connectedGlove = this.gameObject.GetComponent <SG_SenseGloveHardware>();
if (connectedGlove == null && this.transform.parent != null) //still nothing
{
connectedGlove = this.transform.parent.GetComponent <SG_SenseGloveHardware>();
}
}
SG_Util.CheckForHandInfo(this.transform, ref this.handModel);
}
/// <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>
/// Convert an array of float[3] positions taken from the DLL into a Vector3[].
/// </summary>
/// <param name="pos"></param>
/// <returns></returns>
public static Vector3[] ToUnityPosition(SenseGloveCs.Kinematics.Vect3D[] pos)
{
if (pos != null)
{
Vector3[] res = new Vector3[pos.Length];
for (int f = 0; f < pos.Length; f++)
{
res[f] = SG_Util.ToUnityPosition(pos[f]);
}
return(res);
}
return(new Vector3[] { });
}
/// <summary> Check for relevant linked scripts for this HandAnimator, specifically to the SG_HandModelInfo. </summary>
protected override void CheckForScripts()
{
base.CheckForScripts();
SG_Util.CheckForHandInfo(this.transform, ref this.handModelInfo);
if (this.foreArmTransfrom == null)
{
this.foreArmTransfrom = handModelInfo.foreArmTransform;
}
if (this.wristTransfrom == null)
{
this.wristTransfrom = handModelInfo.wristTransform;
}
}
//-------------------------------------------------------------------------------------------------------------------------------------------
// Functions
/// <summary> Setup the SG_BasicFeedback script components </summary>
public virtual void SetupSelf()
{
//Debug.Log(this.name + "Calculated Offsets!");
SG_Util.TryAddRB(this.gameObject, false, true);
cooldownTimer = 0;
SetTrackingTarget(this.trackingTarget, true); //updates offsets on start
this.lastPosition = this.transform.position;
if (this.linkedGlove == null && this.feedbackScript != null && this.feedbackScript.TrackedHand != null)
{
this.linkedGlove = this.feedbackScript.TrackedHand.gloveHardware;
}
}
/// <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> Assign the joints of this script so that the SG_HandAnimator script takes over animation. </summary>
protected override void CollectFingerJoints()
{
SG_Util.CheckForHandInfo(this.transform, ref this.handModelInfo);
if (this.handModelInfo != null)
{
this.fingerJoints = handModelInfo.FingerJoints;
if (this.foreArmTransfrom == null)
{
this.foreArmTransfrom = handModelInfo.foreArmTransform;
}
if (this.wristTransfrom == null)
{
this.wristTransfrom = handModelInfo.wristTransform;
}
}
}
/// <summary> Play an effect using the Thumper module on this glove (if it has any). </summary>
/// <param name="effect"></param>
/// <returns></returns>
public bool SendThumperCmd(SenseGloveCs.ThumperEffect effect)
{
if (GloveData.firmwareVersion >= 4.6f)
{
nextThump = (int)effect; //added to queue
Debug.Log("Queueing " + nextThump.ToString());
return(true);
}
else if (linkedGlove != null && linkedGlove.IsConnected())
{
if (SG_Util.Average(lastBrakeLvls) < thumpFFBThreshold)
{
return(linkedGlove.SendThumperCmd(effect));
}
}
return(false);
}
/// <summary> Extract right-handed coordinate system data from the SenseGlove DLL and convert it into Unity values. </summary>
/// <param name="data"></param>
/// <param name="packets"></param>
/// <param name="totalCSteps"></param>
/// <param name="currCStep"></param>
public SenseGlove_Data(SenseGloveCs.GloveData data)
{
if (data != null)
{
this.gloveSide = SenseGlove_Data.GetSide(data.kinematics.isRight);
this.deviceID = data.deviceID;
this.firmwareVersion = data.firmwareVersion;
this.gloveVersion = data.deviceVersion;
this.packetsPerSecond = data.samplesPerSec;
this.commonOriginPos = SG_Util.ToUnityPosition(data.kinematics.gloveRelPos);
this.commonOriginRot = SG_Util.ToUnityQuaternion(data.kinematics.gloveRelRot);
this.UpdateVariables(data);
}
}
// Use this for initialization
void Start()
{
SG_Util.SetChildren(leftHand.transform, false);
SG_Util.SetChildren(rightHand.transform, false);
if (objectText != null)
{
objectText.text = "";
}
if (nextButton != null)
{
Text btnText = nextButton.GetComponentInChildren <Text>();
if (btnText != null)
{
btnText.text = btnText.text + "\r\n(" + this.nextObjKey.ToString() + ")";
}
}
if (previousButton != null)
{
Text btnText = previousButton.GetComponentInChildren <Text>();
if (btnText != null)
{
btnText.text = btnText.text + "\r\n(" + this.prevObjKey.ToString() + ")";
}
}
if (wristButton != null)
{
Text btnText = wristButton.GetComponentInChildren <Text>();
if (btnText != null)
{
btnText.text = btnText.text + "\r\n(" + this.calibrateWristKey.ToString() + ")";
}
}
ButtonsActive = false;
breakables = new SG_Breakable[ffbObjects.Length];
for (int i = 0; i < ffbObjects.Length; i++)
{
ffbObjects[i].gameObject.SetActive(true); //allows them to call awake at least once.
breakables[i] = ffbObjects[i].GetComponent <SG_Breakable>();
ffbObjects[i].gameObject.SetActive(false);
}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
// Dynamics Methods
#region Dynamics
/// <summary> Update the dynamics (velocity, angular velocity) of the grabreference. </summary>
protected virtual void UpdateDynamics()
{
Vector3 currPos = this.grabReference != null ? this.grabReference.transform.position : this.transform.position;
Quaternion currRot = this.grabReference != null ? this.grabReference.transform.rotation : this.transform.rotation;
Vector3 velocity = (currPos - lastPosition) / Time.deltaTime;
Vector3 angularVelocity = SG_Util.CalculateAngularVelocity(currRot, lastRotation, Time.deltaTime);
this.velocities.Add(velocity);
this.angularVelocities.Add(angularVelocity);
if (velocities.Count > SG_GrabScript.maxDataPoints)
{
this.velocities.RemoveAt(0);
this.angularVelocities.RemoveAt(0);
}
lastPosition = currPos;
lastRotation = currRot;
}
/// <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));
}
/// <summary> Fill the appropriate unity Quaternion and Vector3 arrays based on a single joing chain (finger or glove semgent) </summary>
/// <param name="chain"></param>
/// <param name="positions"></param>
/// <param name="angles"></param>
/// <param name="rotations"></param>
/// <param name="lengths"></param>
protected static void GetLinkVariables(ref SenseGloveCs.Kinematics.JointChain chain, ref Vector3[] positions, ref Vector3[] angles, ref Quaternion[] rotations, ref Vector3[] lengths)
{
int n = chain.joints.Length;
positions = new Vector3[n];
angles = new Vector3[n];
rotations = new Quaternion[n];
lengths = new Vector3[n - 1];
for (int j = 0; j < n; j++)
{
positions[j] = SG_Util.ToUnityPosition(chain.joints[j].position);
angles[j] = SG_Util.ToUnityEuler(chain.joints[j].relativeAngle);
rotations[j] = SG_Util.ToUnityQuaternion(chain.joints[j].rotation);
if (j < n - 1)
{
lengths[j] = SG_Util.ToUnityPosition(chain.lengths[j]);
}
}
}
