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; } } }