public override void Gesture_IdleInterior()
{
if (m_mode == BaseTool.ToolMode.PRIMARY)
{
//Set rbf value from hand position inside sphere
Vector3 handPosition = transform.InverseTransformPoint(HydraController.Instance.GetHand(m_hand).transform.position);
Quaternion handAngle = Quaternion.LookRotation(handPosition);
float distance = Vector3.Distance(transform.position, handPosition);
double[] input = new double[5] {
handAngle.x, handAngle.y, handAngle.z, handAngle.w, distance
};
double[] output = m_rbf.calculateOutput(input);
string outStr = "";
for (int i = 0; i < output.Length; i++)
{
outStr += output[i].ToString() + " ";
}
Debug.Log(outStr);
int index = 0;
foreach (RBFPlugAttachment plug in m_plugs)
{
plug.SetSliderVals((float)output[index++]);
}
}
base.Gesture_IdleInterior();
}
void Update()
{
if (m_arduino.Connected)
{
for (int i = 0; i < m_bendValues.Length; i++)
{
m_bendValues[i] = (double)m_arduino.analogRead(m_bendPins[i]);
}
if (m_toggleCalibration)
{
m_toggleCalibration = false;
m_toggleNextGestureCalibration = true;
m_calibrationState = CalibrationState.CALIBRATING;
m_calibratingGestureIndex = 0;
Debug.Log("Glove calibration start:");
//Set first gesture that needs to be calibrated
m_activeGesture = m_gestures[m_calibratingGestureIndex];
Debug.Log("Calibrate " + m_gestures[m_calibratingGestureIndex]);
}
//Calibration
switch (m_calibrationState)
{
case CalibrationState.AWAITING_CALIBRATION:
if (Convert.ToBoolean(m_arduino.digitalRead(m_bendCalibratePin)) || Input.GetKeyDown(KeyCode.RightArrow))
{
m_calibrationState = CalibrationState.CALIBRATING;
}
break;
case CalibrationState.CALIBRATING:
if (m_toggleNextGestureCalibration)
{
//Start recording samples to calculate the averages per bend sensor.
if (m_currentCalibrationSamples.Count == 0)
{
bIsCollectingSamples = true;
}
if (bIsCollectingSamples)
{
if (m_currentCalibrationSamples.Count < m_calibrationSamples)
{
double[] sensorValues = m_bendValues.Clone() as Double[];
m_currentCalibrationSamples.Add(sensorValues);
//m_rbf.addTrainingPoint(sensorValues, BuildRBFGestureOuput(m_calibratingGestureIndex));
string vals = "";
int count = 0;
foreach (double n in sensorValues)
{
vals += n.ToString() + ", ";
count++;
}
}
else
{
double[] m_calibrationTotal = new double[m_bendValues.Length];
m_calibrationAvg = new double[m_bendValues.Length];
foreach (double[] vals in m_currentCalibrationSamples)
{
for (int i = 0; i < vals.Length; i++)
{
m_calibrationTotal[i] += vals[i];
}
}
for (int i = 0; i < m_calibrationTotal.Length; i++)
{
m_calibrationAvg[i] = m_calibrationTotal[i] / m_calibrationSamples;
}
bIsCollectingSamples = false;
}
}
else
{
//Samples recorded, save into RBF engine.
m_toggleNextGestureCalibration = false;
m_rbf.addTrainingPoint(m_calibrationAvg, BuildRBFGestureOuput(m_calibratingGestureIndex));
m_currentCalibrationSamples.Clear();
m_calibratingGestureIndex++;
if (m_calibratingGestureIndex < m_gestures.Length)
{
m_activeGesture = m_gestures[m_calibratingGestureIndex];
Debug.Log("Calibrate " + m_gestures[m_calibratingGestureIndex]);
}
else
{
m_calibrationState = CalibrationState.CALIBRATED;
m_rbf.calculateWeights();
Debug.Log("Calibration complete!");
}
}
}
break;
case CalibrationState.CALIBRATED:
double[] gestureOutput = m_rbf.calculateOutput(m_bendValues);
//Calculate gesture change velocities
for (int i = 0; i < m_gestures.Length; i++)
{
m_gestureVelocity[i] = gestureOutput[i] - m_lastGestureOutput[i];
if (m_gestureVelocity[i] < 0)
{
m_gestureVelocity[i] *= -1.0;
}
}
m_lastGestureOutput = gestureOutput;
double largestVal = 0.0;
int activeIndex = 0;
for (int i = 0; i < gestureOutput.Length; i++)
{
if (gestureOutput[i] > largestVal)
{
largestVal = gestureOutput[i];
activeIndex = i;
}
}
m_currentGesture = m_gestures[activeIndex];
//Delay the reported gesture change by a frame count to let the RBF settle
if (m_currentGesture != m_lastGesture)
{
m_currentGestureTimer = 0;
}
else
{
m_currentGestureTimer++;
}
if (m_currentGestureTimer > m_gestureSwitchDelay)
{
m_activeGesture = m_currentGesture;
if (m_activeGesture != m_lastGestureDown)
{
m_activeGestureDown = m_activeGesture;
activeGestureVelocity = m_gestureVelocity[activeIndex];
SetDirty();
//SetCollider(activeIndex);
}
else
{
m_activeGestureDown = "";
SetClean();
}
m_lastGestureDown = m_currentGesture;
}
m_lastGesture = m_currentGesture;
break;
}
}
}