#if defined( GESTURE_TRAINER ) #if defined( TARGET_X360 ) #include #elif defined( TARGET_DURANGO ) #include #elif defined( TARGET_ORBIS ) #include "SK_iisu/PS4/include/HumanTrackingCTypes.h" #include "Kinect/PS4/commontypes.h" #else #error unsuported platform #endif #else // defined( GESTURE_TRAINER ) #if defined( _XBOX ) || defined( ITF_X360 ) #include #elif defined( DURANGO ) || defined( ITF_DURANGO ) #include #elif defined( __ORBIS__ ) || defined( ITF_ORBIS ) #include "SK_iisu/PS4/include/HumanTrackingCTypes.h" #include "Kinect/PS4/commontypes.h" #elif defined ( WIN32 ) ||defined( ITF_WIN32) #include #else #error unsuported platform #endif #endif #include #include #include #include #if defined( __ORBIS__ ) || defined( TARGET_ORBIS ) || defined( ITF_ORBIS ) #else #pragma warning(disable : 4793) #endif #include "MuscleTracking.h" #if defined( DURANGO ) || defined( TARGET_DURANGO ) using namespace Microsoft::Xbox::Input::Nui; using namespace DirectX; #endif namespace KinectGesture { #ifdef MUSCLE_DEBUG #define MUSCLE_ASSERT(stmt) {if(!(stmt)){OutputDebugStringA("Assert Failed: " #stmt);int*bad_ptr=NULL;*bad_ptr=0xBAADF00D;}} #else #define MUSCLE_ASSERT(stmt) #endif #define MUSCLE_TODO() MUSCLE_ASSERT("MUSCLE_TODO"&&false) // Implementation: typedef GESTURE_JOINT_INDEX MINDEX; #define MCOUNT GESTURE_JOINT_COUNT #define GRAVITY (-9.8f) #define FAKE_JOINT_HIP_FORWARD ((GESTURE_JOINT_INDEX)(GESTURE_JOINT_COUNT+1)) #define FAKE_JOINT_SHOULDER_FORWARD ((GESTURE_JOINT_INDEX)(GESTURE_JOINT_COUNT+2)) /************************************************************************/ // The density estimate of each segment is based off of // standard tables (http://www.univie.ac.at/cga/teach-in/inverse-dynamics.html) // and by measuring my body (Rahul Agarwal) as seen by the Natal. // Note that for some segments (such as head, feet), the Natal segments are // shorter than actuall body segments /************************************************************************/ #if defined( DURANGO ) || defined( TARGET_DURANGO ) const MUSCLE_DESC GMuscleDescs[MCOUNT] = { /* Endpoint | Inflection | Axis | Basis | Density*/ { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, FAKE_JOINT_HIP_FORWARD, GESTURE_JOINT_SPINE_BASE, 5.61f }, { GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_BASE, FAKE_JOINT_HIP_FORWARD, GESTURE_JOINT_SPINE_BASE, 5.61f }, { GESTURE_JOINT_NECK, GESTURE_JOINT_HEAD, FAKE_JOINT_HIP_FORWARD, GESTURE_JOINT_SPINE_SHOULDER, 30.61f }, { GESTURE_JOINT_HEAD, GESTURE_JOINT_SPINE_SHOULDER, FAKE_JOINT_SHOULDER_FORWARD, GESTURE_JOINT_SPINE_SHOULDER, 29.87f }, { GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_SHOULDER, 35.61f }, { GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_LEFT, 7.23f }, { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_ELBOW_LEFT, 6.66f }, { GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_WRIST_LEFT, 1.91f }, { GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_SHOULDER, 35.61f }, { GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_RIGHT, 7.23f }, { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, 6.66f }, { GESTURE_JOINT_HAND_RIGHT, GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_WRIST_RIGHT, 2.91f }, { GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_BASE, 35.61f }, { GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_LEFT, 16.61f }, { GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_KNEE_LEFT, 5.055f }, { GESTURE_JOINT_FOOT_LEFT, GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_ANKLE_LEFT, 8.33f }, { GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_BASE, 35.61f }, { GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_RIGHT, 16.61f }, { GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_KNEE_RIGHT, 5.055f }, { GESTURE_JOINT_FOOT_RIGHT, GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_ANKLE_RIGHT, 8.33f }, { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_NECK, FAKE_JOINT_SHOULDER_FORWARD, GESTURE_JOINT_SPINE_MID, 5.61f }, { GESTURE_JOINT_HAND_TIP_LEFT, GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_HAND_LEFT, 1.91f }, { GESTURE_JOINT_THUMB_LEFT, GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_HAND_LEFT, 1.91f }, { GESTURE_JOINT_HAND_TIP_RIGHT, GESTURE_JOINT_HAND_RIGHT, GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_HAND_RIGHT, 1.91f }, { GESTURE_JOINT_THUMB_RIGHT, GESTURE_JOINT_HAND_RIGHT, GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_HAND_RIGHT, 1.91f }, }; #else const MUSCLE_DESC GMuscleDescs[MCOUNT] = { /* Endpoint | Inflection | Axis | Basis | Density*/ { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, FAKE_JOINT_HIP_FORWARD, GESTURE_JOINT_SPINE_BASE, 5.61f }, { GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_BASE, FAKE_JOINT_HIP_FORWARD, GESTURE_JOINT_SPINE_BASE, 5.61f }, { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SPINE_MID, FAKE_JOINT_HIP_FORWARD, GESTURE_JOINT_SPINE_MID, 30.61f }, { GESTURE_JOINT_HEAD, GESTURE_JOINT_SPINE_SHOULDER, FAKE_JOINT_SHOULDER_FORWARD, GESTURE_JOINT_SPINE_SHOULDER, 29.87f }, { GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_SHOULDER, 35.61f }, { GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_LEFT, 7.23f }, { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_ELBOW_LEFT, 6.66f }, { GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_WRIST_LEFT, 1.91f }, { GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_SHOULDER, 35.61f }, { GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_RIGHT, 7.23f }, { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, 6.66f }, { GESTURE_JOINT_HAND_RIGHT, GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_WRIST_RIGHT, 2.91f }, { GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_BASE, 35.61f }, { GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_LEFT, 16.61f }, { GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_KNEE_LEFT, 5.055f }, { GESTURE_JOINT_FOOT_LEFT, GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_ANKLE_LEFT, 8.33f }, { GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_BASE, 35.61f }, { GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_RIGHT, 16.61f }, { GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_KNEE_RIGHT, 5.055f }, { GESTURE_JOINT_FOOT_RIGHT, GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_ANKLE_RIGHT, 8.33f }, }; #endif const MUSCLE_DESC* MuscleDescriptions() { return GMuscleDescs; } class MuscleCalculator { private: const XMVECTOR* m_pJoints; const GESTURE_JOINT_TRACKING_STATE* m_pStates; const MUSCLE_FRAME* m_pPrevious; float m_fDeltaSeconds; MUSCLE_FRAME* m_pResult; //float m_fLeftRightScale; //float m_fGlobalForceScale; bool m_bIsFirstFrame; public: MuscleCalculator( const XMVECTOR* pJoints, const GESTURE_JOINT_TRACKING_STATE* pStates, const MUSCLE_FRAME* pPrevious, const float deltaSeconds, MUSCLE_FRAME* pResult, bool firstFrame) : m_pJoints(pJoints), m_pStates(pStates), m_pPrevious(pPrevious), m_fDeltaSeconds(deltaSeconds), m_pResult(pResult), m_bIsFirstFrame(firstFrame) { }; ~MuscleCalculator() { } private: //************************************ // Method: readJointPosition // This method returns the position of joint IND by looking it up in pJoints. // It handles the case of looking for the joints FAKE_JOINT_SHOULDER_FORWARD, // and FAKE_JOINT_HIP_FORWARD, which are computed as they are not actually stored anywhere //************************************ XMVECTOR readJointPosition(const XMVECTOR* pJoints, int ind) { if (ind < GESTURE_JOINT_COUNT) { return pJoints[ind]; } //Optional: Makes lower body torques more stable if (ind == GESTURE_JOINT_FOOT_LEFT) return m_pJoints[GESTURE_JOINT_ANKLE_LEFT] + XMVectorSet(0, 0, -0.1f, 0.0f); if (ind == GESTURE_JOINT_FOOT_RIGHT) return m_pJoints[GESTURE_JOINT_ANKLE_RIGHT] + XMVectorSet(0, 0, -0.1f, 0.0f); static float forwardOffset = 1.0f; XMVECTOR across = XMVectorSet(1.0f, 0.0f, 0.0f, 0.0f), center = pJoints[GESTURE_JOINT_SPINE_BASE]; if (ind == FAKE_JOINT_HIP_FORWARD) { across = pJoints[GESTURE_JOINT_HIP_RIGHT] - pJoints[GESTURE_JOINT_HIP_LEFT]; center = pJoints[GESTURE_JOINT_SPINE_BASE]; } else if (ind == FAKE_JOINT_SHOULDER_FORWARD) { across = pJoints[GESTURE_JOINT_SHOULDER_RIGHT] - pJoints[GESTURE_JOINT_SHOULDER_LEFT]; center = pJoints[GESTURE_JOINT_SPINE_SHOULDER]; } XMVECTOR trunk = readJointPosition(pJoints, GESTURE_JOINT_SPINE_SHOULDER) - readJointPosition(pJoints, GESTURE_JOINT_SPINE_BASE); XMVECTOR forward = -XMVector3Cross(trunk, across); return (forwardOffset * XMVector3NormalizeEst(forward)) + center; } //************************************ // Method: IsLowerBody // Used during force normalization. //************************************ static bool IsLowerBody(int joint) { switch (joint) { case GESTURE_JOINT_HIP_LEFT: case GESTURE_JOINT_HIP_RIGHT: case GESTURE_JOINT_KNEE_LEFT: case GESTURE_JOINT_KNEE_RIGHT: case GESTURE_JOINT_ANKLE_LEFT: case GESTURE_JOINT_ANKLE_RIGHT: case GESTURE_JOINT_FOOT_LEFT: case GESTURE_JOINT_FOOT_RIGHT: return true; default: return false; }; } //************************************ // Method: CalculateSensorToMuscleMatrix // Calculates the transformation matrix from // sensor space to muscle space for a given muscle. // There are many special cases to help improve stability // (ex. cases where all 3 joints become colinear) and // to conform to conventions. // // The X direction in muscle space corresponds to "pull" muscle action // The Y direction in muscle space corresponds to "twist" muscle action // This follows the convention that bring the endpoint forward or out of // the body is an action in the positive direction. This is enforced by // flipping the axes of certain segments //************************************ XMMATRIX CalculateSensorToMuscleMatrix(MUSCLE_DESC desc) { XMVECTOR endPos = readJointPosition(m_pJoints, desc.EndPoint); XMVECTOR inflection = readJointPosition(m_pJoints, desc.Inflection); XMVECTOR axisJoint = readJointPosition(m_pJoints, desc.Axis); XMVECTOR yDir = axisJoint - inflection; XMVECTOR xDir = XMVector3Cross(endPos - inflection, yDir); XMVECTOR other = XMVectorZero(); switch (desc.EndPoint) { case GESTURE_JOINT_ELBOW_RIGHT: yDir *= -1; case GESTURE_JOINT_ELBOW_LEFT: other = readJointPosition(m_pJoints, GESTURE_JOINT_SPINE_BASE); xDir = XMVector3Cross(XMVector3Normalize(inflection - axisJoint), XMVector3Normalize(axisJoint - other)); break; case GESTURE_JOINT_SHOULDER_RIGHT: yDir *= -1; case GESTURE_JOINT_SHOULDER_LEFT: xDir *= -1; break; case GESTURE_JOINT_ANKLE_LEFT: yDir *= -1; case GESTURE_JOINT_ANKLE_RIGHT: //If the segments are almost collinear, use a reference segment to compute the matrix if ((XM_PI - XMVectorGetX(XMVector3AngleBetweenVectors(axisJoint - inflection, endPos - inflection))) < XM_PI / 8) { other = readJointPosition(m_pJoints, FAKE_JOINT_HIP_FORWARD) - readJointPosition(m_pJoints, GESTURE_JOINT_SPINE_BASE); xDir = XMVector3Cross(inflection - axisJoint, other); } else { xDir *= -1; } break; case GESTURE_JOINT_KNEE_LEFT: case GESTURE_JOINT_KNEE_RIGHT: //xDir *= -1; //other = XMVector3Normalize(inflection - endPos); //yDir = yDir - XMVector3Dot(yDir, other) * other; //if( (XM_PI - XMVectorGetX(XMVector3AngleBetweenVectors(axisJoint - inflection, endPos - inflection))) < XM_PI / 8 ) yDir = XMVector3Cross(endPos - inflection, xDir); other = readJointPosition(m_pJoints, GESTURE_JOINT_SPINE_SHOULDER) - readJointPosition(m_pJoints, GESTURE_JOINT_SPINE_BASE); xDir = XMVector3Cross(inflection - axisJoint, other); if ((XM_PI - XMVectorGetX(XMVector3AngleBetweenVectors(endPos - inflection, other))) < XM_PI / 8) { yDir = XMVector3Cross(endPos - inflection, xDir); if (desc.EndPoint == GESTURE_JOINT_KNEE_RIGHT) yDir *= -1; } else yDir = XMVector3Cross(endPos - inflection, other); break; case GESTURE_JOINT_FOOT_RIGHT: yDir *= -1; case GESTURE_JOINT_FOOT_LEFT: xDir *= -1; if (XMVector3Equal(xDir, XMVectorZero())) { other = readJointPosition(m_pJoints, GESTURE_JOINT_SPINE_BASE); XMVECTOR other2 = readJointPosition(m_pJoints, FAKE_JOINT_HIP_FORWARD); xDir = XMVector3Cross(other2 - other, axisJoint - inflection); } break; case GESTURE_JOINT_WRIST_RIGHT: yDir *= -1.0f; break; default:break; // Gets rid of 'warning : x enumeration values not handled in switch:...' } XMVECTOR finalDir = XMVector3Cross(xDir, yDir); if (XMVector3Equal(yDir, XMVectorZero()) || XMVectorGetX(XMVector3LengthSq(xDir)) < 0.00000001f) return XMMatrixIdentity(); return XMMatrixLookToRH(XMVectorZero(), -finalDir, yDir); } static void AddToExternalCOM(MUSCLE_DATA* pInto, XMVECTOR pos, XMVECTOR vel, float mass) { if (mass != 0.0f) { float totalWeight = pInto->mLimbMassExternal + mass; pInto->mLimbExternalCOM = ( (pInto->mLimbExternalCOM * pInto->mLimbMassExternal) + (pos * mass)) / totalWeight; pInto->mLimbExternalCOMVelocity = ( (pInto->mLimbExternalCOMVelocity * pInto->mLimbMassExternal) + (vel * mass)) / totalWeight; pInto->mLimbMassExternal = totalWeight; } } //************************************ // Method: ProcessGeneral // This method does all the "heavy lifting" of computing the // torques and forces for each muscle // Parameter: bool propagateExternalForce // This parameter is used to avoid propagating // ground reaction forces to the hips, // in order to make the annotation look better //************************************ void ProcessGeneral(int mi, bool propagateExternalForce = true) { MUSCLE_DATA* pMD = &(m_pResult->Muscles[mi]); const MUSCLE_DATA* pPM = &(m_pPrevious->Muscles[mi]); MUSCLE_DESC desc = GMuscleDescs[mi]; MUSCLE_ASSERT(desc.EndPoint == mi); // ensure that the joints are indexed by their end-point XMVECTOR endPos = readJointPosition(m_pJoints, mi); XMVECTOR inflection = readJointPosition(m_pJoints, desc.Inflection); XMVECTOR axisJoint = readJointPosition(m_pJoints, desc.Axis); if ((m_pStates != NULL) && (m_pStates[mi] == GESTURE_JOINT_NOT_TRACKED)) { #ifdef _XBOX XMemSet(pMD, 0, sizeof(*pMD)); #else memset(pMD, 0, sizeof(*pMD)); #endif pMD->mJointResolutionRadius = 4.0f; // somewhere within the sensor radius, 4 meters return; } XMVECTOR r = endPos - inflection; if ((mi == GESTURE_JOINT_SHOULDER_LEFT) || (mi == GESTURE_JOINT_SHOULDER_RIGHT)) { // For the shoulders, simulate the effect of the muscles which hold up the shoulder bone by lowering the distance // from the clavical to the shoulder: r *= 0.3f; } // Calculate velocity and acceleration: const float fullyTrackedRadius = 0.05f; bool isFullyTracked = m_pStates ? (m_pStates[mi] == GESTURE_JOINT_TRACKED) : true; bool wasFullyTracked = (pPM->mJointResolutionRadius == fullyTrackedRadius); pMD->mJointPosition = endPos; pMD->mJointResolutionRadius = isFullyTracked ? fullyTrackedRadius : 0.2f; // noise radius, default is 5 cm UpdateLinearVelocityAndAcceleration(pMD, pPM); //Update angular quantities pMD->mLimbAngularAcceleration = XMVector3Cross(0.5 * r, pMD->mLimbLinearAcceleration); pMD->mLimbAngularVelocity = XMVector3Cross(0.5 * r, pMD->mLimbLinearVelocity); pMD->mLimbAngularExtension = XMVectorGetX(XMVector3AngleBetweenVectors(endPos - inflection, axisJoint - inflection)); pMD->mLimbLength = XMVectorGetX(XMVector3Length(r)); if (!isFullyTracked || !wasFullyTracked) { static float reduceInfered = 0.0f; pMD->mLimbLinearVelocity *= reduceInfered; pMD->mLimbLinearAcceleration *= reduceInfered; pMD->mLimbAngularVelocity *= reduceInfered; pMD->mLimbAngularAcceleration *= reduceInfered; pMD->mLimbTempJerk *= reduceInfered; pMD->mLimbTempAcceleration *= reduceInfered; } // Calculate muscle orientation: XMVECTOR det; XMMATRIX sensorToMuscle = CalculateSensorToMuscleMatrix(desc); XMMATRIX muscleToSensor = XMMatrixInverse(&det, sensorToMuscle); #if defined( __ORBIS__ ) || defined( ITF_ORBIS ) pMD->mMuscleToSensorX = XMVectorSet(muscleToSensor.val[0][0], muscleToSensor.val[0][1], muscleToSensor.val[0][2], muscleToSensor.val[0][3]); pMD->mMuscleToSensorY = XMVectorSet(muscleToSensor.val[1][0], muscleToSensor.val[1][1], muscleToSensor.val[1][2], muscleToSensor.val[1][3]); pMD->mMuscleToSensorZ = XMVectorSet(muscleToSensor.val[2][0], muscleToSensor.val[2][1], muscleToSensor.val[2][2], muscleToSensor.val[2][3]); #else pMD->mMuscleToSensorX = muscleToSensor.r[0]; pMD->mMuscleToSensorY = muscleToSensor.r[1]; pMD->mMuscleToSensorZ = muscleToSensor.r[2]; #endif // Estimate mass: pMD->mLimbMassEstimate = XMVectorGetX(XMVector3Length(r)) * desc.Density; MUSCLE_ASSERT(pMD->mLimbMassEstimate != 0.0f); float momentOfInertiaEst = pMD->mLimbMassEstimate * 1.0f / 3.0f * XMVectorGetX(XMVector3LengthSq(r)); //Moment of inertia of a thin rod around 1 end // Retrieve external forces: XMVECTOR externalForce = pMD->mDynamicExternalForce; XMVECTOR externalTorque = pMD->mDynamicExternalTorque; XMVECTOR weight = XMVectorSet(0, GRAVITY, 0, 0) * pMD->mLimbMassEstimate; float propagationDir = 1.0f; if (IsLowerBody(mi) && (XMVectorGetY(externalForce) < 0.0f)) { propagationDir = -1.0f; } //Compute Torques XMVECTOR netTorque = momentOfInertiaEst * pMD->mLimbAngularAcceleration; XMVECTOR gravityTorque = XMVector3Cross(0.5f * r, weight); XMVECTOR missingTorque = netTorque - gravityTorque - XMVector3Cross(r, externalForce) - externalTorque; pMD->mDynamicLocalTorqueBalance = missingTorque; pMD->mDynamicLocalTorqueMotion = XMVectorZero(); //Static torques XMVECTOR missingStaticTorque = -gravityTorque - XMVector3Cross(r, pMD->mStaticExternalForce) - pMD->mStaticExternalTorque; pMD->mStaticLocalTorqueBalance = missingStaticTorque; // Now figure out what the additional force must have been to account for the change in velocity of the center of mass: XMVECTOR dynLocalForce = (pMD->mLimbLinearAcceleration * pMD->mLimbMassEstimate) - (weight * propagationDir) - externalForce; XMVECTOR muscleStaticForce = -(weight + pMD->mStaticExternalForce); if (propagateExternalForce) { MUSCLE_DATA* pInto = &(m_pResult->Muscles[desc.Inflection]); pInto->mDynamicExternalForce += -dynLocalForce; pInto->mDynamicExternalTorque += -missingTorque; pInto->mStaticExternalTorque += -missingStaticTorque; pInto->mStaticExternalForce += -muscleStaticForce; AddToExternalCOM(pInto, pMD->mJointPosition + r*0.5f, pMD->mLimbLinearVelocity, pMD->mLimbMassEstimate); AddToExternalCOM(pInto, pMD->mLimbExternalCOM, pMD->mLimbExternalCOMVelocity, pMD->mLimbMassExternal); } // Transform the dynamic torque pMD->mMuscleDynamicLocalTorque = XMVectorSet( XMVectorGetX(XMVector3Dot(missingTorque, pMD->mMuscleToSensorX)), XMVectorGetX(XMVector3Dot(missingTorque, pMD->mMuscleToSensorY)), XMVectorGetX(XMVector3Dot(missingTorque, pMD->mMuscleToSensorZ)), 0); } //************************************ // Method: EstimateCenterOfMass // Calculates the center of mass. Makes the assumption // that the CoM for each segment is in its center. //************************************ void EstimateCenterOfMass() { m_pResult->CenterOfMass.mLimbMassEstimate = 0.0f; m_pResult->CenterOfMass.mJointPosition = XMVectorZero(); for (int i = 0; i < MCOUNT; i++) { MUSCLE_DESC desc = GMuscleDescs[i]; XMVECTOR endPos = readJointPosition(m_pJoints, desc.EndPoint); XMVECTOR inflection = readJointPosition(m_pJoints, desc.Inflection); float mass = desc.Density * XMVectorGetX(XMVector3Length(endPos - inflection)); m_pResult->CenterOfMass.mLimbMassEstimate += mass; m_pResult->CenterOfMass.mJointPosition += mass * ((endPos + inflection) * 0.5f); } if (m_pResult->CenterOfMass.mLimbMassEstimate == 0.0f) m_pResult->CenterOfMass.mJointPosition = XMVectorZero(); else m_pResult->CenterOfMass.mJointPosition /= m_pResult->CenterOfMass.mLimbMassEstimate; const MUSCLE_DATA* prevCM = m_bIsFirstFrame ? &m_pResult->CenterOfMass : &m_pPrevious->CenterOfMass; UpdateLinearVelocityAndAcceleration(&m_pResult->CenterOfMass, prevCM); } //************************************ // These methods do a rough estimate of the forces on each foot by projecting the center of mass onto the // X-Z plane. Note that the Z position is actually discarded, as the z-postion of the feet was found to be highly // unreliable (the Natal often made it appear as if the player was leaning forwards or backwards) //************************************ void EstimateFootForces() { float massEstimate = m_pResult->CenterOfMass.mLimbMassEstimate; XMVECTOR vDynFootForce = massEstimate * (XMVectorSet(0, -GRAVITY, 0, 0) - m_pResult->CenterOfMass.mLimbLinearAcceleration); XMVECTOR vStcFootForce = massEstimate * (XMVectorSet(0, -GRAVITY, 0, 0)); m_pResult->CenterOfMass.mDynamicExternalForce = vDynFootForce; m_pResult->CenterOfMass.mStaticExternalForce = vStcFootForce; float weightNotFlying; float weightDistribution; EstimateWeightDistributionLR(&weightDistribution, &weightNotFlying); float weightDistributionLeft = weightDistribution * weightNotFlying; float weightDistributionRight = (1 - weightDistribution) * weightNotFlying; const float minBodyPercentOnLegs = 0.618f; // even when there is no weight on a leg, use this amount of the mass of the body for normalization m_pResult->Muscles[GESTURE_JOINT_FOOT_LEFT].mDynamicExternalForce += weightDistributionLeft * vDynFootForce; m_pResult->Muscles[GESTURE_JOINT_FOOT_RIGHT].mDynamicExternalForce += weightDistributionRight * vDynFootForce; m_pResult->Muscles[GESTURE_JOINT_FOOT_LEFT].mStaticExternalForce += weightDistributionLeft * vStcFootForce; m_pResult->Muscles[GESTURE_JOINT_FOOT_RIGHT].mStaticExternalForce += weightDistributionRight * vStcFootForce; AddToExternalCOM(&(m_pResult->Muscles[GESTURE_JOINT_FOOT_LEFT]), m_pResult->CenterOfMass.mJointPosition, m_pResult->CenterOfMass.mLimbLinearVelocity, gesture_max(weightDistributionLeft, minBodyPercentOnLegs) * massEstimate); AddToExternalCOM(&(m_pResult->Muscles[GESTURE_JOINT_FOOT_RIGHT]), m_pResult->CenterOfMass.mJointPosition, m_pResult->CenterOfMass.mLimbLinearVelocity, gesture_max(weightDistributionRight, minBodyPercentOnLegs) * massEstimate); } void EstimateWeightDistributionLR(float* outPercentLeft, float* outPercentNotFlying) { //Set up a plane to minimize distortions XMVECTOR referenceAxis = XMVectorSet(1, 0, 0, 0); XMVECTOR referenceNormal = XMVectorSet(0, 0, -1.0f, 0); referenceAxis = XMVectorSetY(referenceAxis, 0); referenceNormal = XMVectorSetY(referenceNormal, 0); XMVECTOR leftHeel = readJointPosition(m_pJoints, GESTURE_JOINT_ANKLE_LEFT); XMVECTOR rightHeel = readJointPosition(m_pJoints, GESTURE_JOINT_ANKLE_RIGHT); float distributionLeft; XMVECTOR vCMAccel = m_pResult->CenterOfMass.mLimbLinearAcceleration; XMVECTOR vFootDisp = leftHeel - rightHeel; XMVECTOR CMDisplacement = m_pResult->CenterOfMass.mJointPosition - rightHeel; float d_tot = fabsf(XMVectorGetX(XMVector3Dot(vFootDisp, referenceAxis))); float d_r = gesture_max(-XMVectorGetX(XMVector3Dot(CMDisplacement, referenceAxis)), 0.0f); XMVECTOR projectedAccel = vCMAccel - XMVector3Dot(vCMAccel, referenceNormal) * referenceNormal; XMVECTOR projectedCM = CMDisplacement - XMVector3Dot(CMDisplacement, referenceNormal) * referenceNormal; float netTorque = XMVectorGetX(XMVector3Dot(XMVector3Cross(projectedCM, projectedAccel), referenceNormal)); float force_left = (netTorque + d_r * -GRAVITY) / d_tot; distributionLeft = force_left / (XMVectorGetX(XMVector3Length(projectedAccel)) - GRAVITY); distributionLeft = gesture_max(gesture_min(distributionLeft, 1.0f), 0.0f); // Special case: if one of the knees is bent a lot more than the other, then the weight // of the body must be on the straighter knee. XMVECTOR refDown = XMVectorSet(0, -1, 0, 0); const float angleForBent = 3.14149f / 6.0f; float angleLeftDown = XMVectorGetX(XMVector3AngleBetweenVectors(refDown, readJointPosition(m_pJoints, GESTURE_JOINT_KNEE_LEFT) - readJointPosition(m_pJoints, GESTURE_JOINT_HIP_LEFT))); float angleRightDown = XMVectorGetX(XMVector3AngleBetweenVectors(refDown, readJointPosition(m_pJoints, GESTURE_JOINT_KNEE_RIGHT) - readJointPosition(m_pJoints, GESTURE_JOINT_HIP_RIGHT))); if ((angleRightDown - angleLeftDown) > angleForBent) distributionLeft = 1.0f; if ((angleLeftDown - angleRightDown) > angleForBent) distributionLeft = 0.0f; *outPercentLeft = distributionLeft; *outPercentNotFlying = 1.0f; } //************************************ // Method: UpdateLinearVelocityAndAcceleration // This function uses a Newmark multivalue integration scheme // that is modified to apply simultaneous differentiation and smoothing // based on this paper: http://www.springerlink.com/content/90134383w2027768/ // The method is applied twice, once to get accurate velocity, and once to // get accurate accelerations. The "jerk" is the term for the 3rd derivative // or position with respect to time //************************************ void UpdateLinearVelocityAndAcceleration(MUSCLE_DATA* curFrame, const MUSCLE_DATA* prevFrame) { static float beta1 = 100.0f; static float gamma1 = 20.0f; static float beta2 = 6.0f; static float gamma2 = 2.0f; float beta1DtLimit = sqrtf(beta1) / (1.3f * 30.0f); float gama1DtLimit = beta1 / (1.5f * 30.0f*gamma1); float dt1Limit = gesture_min(beta1DtLimit, gama1DtLimit); float fDeltaSeconds1Limitted = gesture_min(m_fDeltaSeconds, dt1Limit); XMVECTOR vInstantaneousLinearVelocity = (curFrame->mJointPosition - prevFrame->mJointPosition) / m_fDeltaSeconds; curFrame->mLimbTempAcceleration = prevFrame->mLimbTempAcceleration + (fDeltaSeconds1Limitted * 30.0f / beta1) * (30.0f * (vInstantaneousLinearVelocity - prevFrame->mLimbLinearVelocity) - (prevFrame->mLimbTempAcceleration * 0.5f)); XMVECTOR vIntermediateJerk = (curFrame->mLimbTempAcceleration - prevFrame->mLimbTempAcceleration) / fDeltaSeconds1Limitted; curFrame->mLimbLinearVelocity = prevFrame->mLimbLinearVelocity + fDeltaSeconds1Limitted * (curFrame->mLimbTempAcceleration + (gamma1 - 1.0f) * vIntermediateJerk / 30.0f); float beta2DtLimit = sqrtf(beta2) / (1.3f * 30.0f); float gama2DtLimit = beta2 / (1.5f * 30.0f * gamma2); float dt2Limit = gesture_min(beta2DtLimit, gama2DtLimit); float fDeltaSeconds2Limitted = gesture_min(m_fDeltaSeconds, dt2Limit); XMVECTOR vLinearVelocityDerivative = (curFrame->mLimbLinearVelocity - prevFrame->mLimbLinearVelocity) / m_fDeltaSeconds; curFrame->mLimbTempJerk = prevFrame->mLimbTempJerk + (fDeltaSeconds2Limitted * 30.0f / beta2) * (30.0f * (vLinearVelocityDerivative - prevFrame->mLimbLinearAcceleration) - (prevFrame->mLimbTempJerk * 0.5f)); XMVECTOR vLinearJerkDerivative = (curFrame->mLimbTempJerk - prevFrame->mLimbTempJerk) / fDeltaSeconds2Limitted; curFrame->mLimbLinearAcceleration = prevFrame->mLimbLinearAcceleration + fDeltaSeconds2Limitted * (curFrame->mLimbTempJerk + (gamma2 - 1.0f) * vLinearJerkDerivative / 30.0f); } //************************************ // Method: NormalizeForces // Calculates a normalization constant for each segment // to make the rendering look pretty //************************************ void NormalizeForces() { float fHalfHeight = 0.5f * XMVectorGetX(XMVector3Length(m_pResult->Muscles[GESTURE_JOINT_SPINE_BASE].mJointPosition - m_pResult->Muscles[GESTURE_JOINT_FOOT_LEFT].mJointPosition)); fHalfHeight += 0.5f * XMVectorGetX(XMVector3Length(m_pResult->Muscles[GESTURE_JOINT_SPINE_BASE].mJointPosition - m_pResult->Muscles[GESTURE_JOINT_FOOT_RIGHT].mJointPosition)); for (int li = 0; li < MCOUNT; li++) { MUSCLE_DATA* curMuscle = &m_pResult->Muscles[li]; curMuscle->mForceNormalizationConstant = fabsf(GRAVITY) * (curMuscle->mLimbMassExternal); curMuscle->mTorqueNormalizationConstant = fabsf(GRAVITY) * (curMuscle->mLimbLength) * (curMuscle->mLimbMassExternal); if (curMuscle->mLimbMassExternal == 0.0f) { curMuscle->mLimbExternalCOM = curMuscle->mJointPosition; } if (curMuscle->mForceNormalizationConstant == 0.0f) { curMuscle->mForceNormalizationConstant = 1.0f; } if (curMuscle->mTorqueNormalizationConstant == 0.0f) { curMuscle->mTorqueNormalizationConstant = 1.0f; } } m_pResult->CenterOfMass.mForceNormalizationConstant = (m_pResult->CenterOfMass.mLimbMassEstimate * -GRAVITY); m_pResult->CenterOfMass.mTorqueNormalizationConstant = fHalfHeight * (m_pResult->CenterOfMass.mLimbMassEstimate * -GRAVITY); } public: void Process() { #ifdef _XBOX XMemSet(m_pResult, 0, sizeof(*m_pResult)); #else memset(m_pResult, 0, sizeof(*m_pResult)); #endif EstimateCenterOfMass(); ProcessGeneral(GESTURE_JOINT_HAND_LEFT); ProcessGeneral(GESTURE_JOINT_WRIST_LEFT); ProcessGeneral(GESTURE_JOINT_ELBOW_LEFT); ProcessGeneral(GESTURE_JOINT_SHOULDER_LEFT); ProcessGeneral(GESTURE_JOINT_HAND_RIGHT); ProcessGeneral(GESTURE_JOINT_WRIST_RIGHT); ProcessGeneral(GESTURE_JOINT_ELBOW_RIGHT); ProcessGeneral(GESTURE_JOINT_SHOULDER_RIGHT); ProcessGeneral(GESTURE_JOINT_HEAD); ProcessGeneral(GESTURE_JOINT_SPINE_SHOULDER); ProcessGeneral(GESTURE_JOINT_SPINE_MID); EstimateFootForces(); ProcessGeneral(GESTURE_JOINT_FOOT_LEFT); ProcessGeneral(GESTURE_JOINT_ANKLE_LEFT); ProcessGeneral(GESTURE_JOINT_KNEE_LEFT); ProcessGeneral(GESTURE_JOINT_HIP_LEFT); ProcessGeneral(GESTURE_JOINT_FOOT_RIGHT); ProcessGeneral(GESTURE_JOINT_ANKLE_RIGHT); ProcessGeneral(GESTURE_JOINT_KNEE_RIGHT); ProcessGeneral(GESTURE_JOINT_HIP_RIGHT); ProcessGeneral(GESTURE_JOINT_SPINE_BASE, false); NormalizeForces(); }; }; XMMATRIX muscleUntiltMatrix(XMVECTOR vToNormal, XMVECTOR vFromNormal) { if (XMVector3Equal(vToNormal, vFromNormal)) { return XMMatrixIdentity(); } else { XMVECTOR axis = XMVector3Cross(vToNormal, vFromNormal); float angle = -XMVectorGetX(XMVector3AngleBetweenNormals(vToNormal, vFromNormal)); return XMMatrixRotationAxis(axis, angle); } } HRESULT MuscleFrameCalculate( __out MUSCLE_FRAME* pResult, const MUSCLE_FRAME* pPrevious, const XMVECTOR* pJoints, const XMVECTOR vJointsUp, const GESTURE_JOINT_TRACKING_STATE* pStates, const float deltaSeconds ) { if (pResult == NULL || pJoints == NULL) return S_FALSE; XMVECTOR untiltedJoints[GESTURE_JOINT_COUNT]; XMMATRIX untilter = muscleUntiltMatrix(XMVectorSet(0, 1, 0, 0), vJointsUp); for (int ji = 0; ji < GESTURE_JOINT_COUNT; ji++) { untiltedJoints[ji] = XMVector3Transform(pJoints[ji], untilter); } pJoints = untiltedJoints; float effectiveDt = deltaSeconds; if (deltaSeconds > 1.0f || deltaSeconds < 0.0f) { // if the time passes is not greater than one second, then essentially // reset the calculation. pPrevious = NULL; effectiveDt = 1.0f / 30.0f; } MUSCLE_FRAME otherPrev; bool firstFrame = false; if (pPrevious == NULL) { #ifdef _XBOX XMemSet(&otherPrev, 0, sizeof(otherPrev)); #else memset(&otherPrev, 0, sizeof(otherPrev)); #endif for (int i = 0; i < GESTURE_JOINT_COUNT; i++) { otherPrev.Muscles[i].mJointPosition = pJoints[i]; } pPrevious = &otherPrev; firstFrame = true; } else if (pPrevious == pResult) { // if the src and dst muscle buffers are the same, make a copy and then use // that as the previous: otherPrev = *pPrevious; pPrevious = &otherPrev; } MuscleCalculator calc(pJoints, pStates, pPrevious, effectiveDt, pResult, firstFrame); calc.Process(); pResult->TimeSpan = deltaSeconds; return S_OK; } //Returns Dynamic torque when joint is speeding up, zero vector otherwise XMVECTOR MuscleFrameGetAcceleratingTorque(const MUSCLE_FRAME* pFrame, GESTURE_JOINT_INDEX index) { if (pFrame == NULL) return XMVectorZero(); const MUSCLE_DATA* pMD = &pFrame->Muscles[index]; float angularPower = XMVectorGetX(XMVector3Dot(pMD->mLimbAngularAcceleration, pMD->mLimbAngularVelocity)); if (angularPower > 0.0f) return (pMD->mDynamicLocalTorqueMotion + pMD->mDynamicLocalTorqueBalance) / pMD->mTorqueNormalizationConstant; return XMVectorZero(); } //Returns Dynamic torque when joint is slowing down, zero vector otherwise XMVECTOR MuscleFrameGetDecceleratingTorque(const MUSCLE_FRAME* pFrame, GESTURE_JOINT_INDEX index) { if (pFrame == NULL) return XMVectorZero(); const MUSCLE_DATA* pMD = &pFrame->Muscles[index]; float angularPower = XMVectorGetX(XMVector3Dot(pMD->mLimbAngularAcceleration, pMD->mLimbAngularVelocity)); if (angularPower < 0.0f) return (pMD->mDynamicLocalTorqueMotion + pMD->mDynamicLocalTorqueBalance) / pMD->mTorqueNormalizationConstant; return XMVectorZero(); } //Returns Dynamic force when joint is speeding up, zero vector otherwise XMVECTOR MuscleFrameGetAcceleratingForce(const MUSCLE_FRAME* pFrame, GESTURE_JOINT_INDEX index) { if (pFrame == NULL) return XMVectorZero(); const MUSCLE_DATA* pMD = &pFrame->Muscles[index]; float linearPower = XMVectorGetX(XMVector3Dot(pMD->mLimbLinearAcceleration, pMD->mLimbAngularVelocity)); if (linearPower > 0.0f) return (pMD->mDynamicExternalForce) / pMD->mForceNormalizationConstant; return XMVectorZero(); } //Returns Dynamic force when joint is slowing down, zero vector otherwise XMVECTOR MuscleFrameGetDecceleratingForce(const MUSCLE_FRAME* pFrame, GESTURE_JOINT_INDEX index) { if (pFrame == NULL) return XMVectorZero(); const MUSCLE_DATA* pMD = &pFrame->Muscles[index]; float linearPower = XMVectorGetX(XMVector3Dot(pMD->mLimbLinearAcceleration, pMD->mLimbAngularVelocity)); if (linearPower < 0.0f) return (pMD->mDynamicExternalForce) / pMD->mForceNormalizationConstant; return XMVectorZero(); } //Returns the dot product of the linear acceleration and the linear velocity float MuscleDataGetLinearPower(const MUSCLE_DATA* pData) { return XMVectorGetX(XMVector3Dot( pData->mLimbLinearAcceleration, pData->mLimbLinearVelocity)); } //Returns the normalized dot products of the normalized external force with the normalized external COM velocity XMVECTOR MuscleFrameGetDynamicPower(const MUSCLE_FRAME* pFrame, GESTURE_JOINT_INDEX index) { const MUSCLE_DATA* pMD = &pFrame->Muscles[index]; float massEx = (pMD->mLimbMassExternal != 0) ? pMD->mLimbMassExternal : 1.0f; float forceNorm = (pMD->mForceNormalizationConstant != 0) ? pMD->mForceNormalizationConstant : 1.0f; return XMVectorMultiply(pMD->mDynamicExternalForce / forceNorm, pMD->mLimbExternalCOMVelocity / massEx) * -1.0f; } // ----------------------------- Muscle Visualization ----------------------------- struct MuscleVisGroup { GESTURE_JOINT_INDEX Origin; GESTURE_JOINT_INDEX EndPoint; GESTURE_JOINT_INDEX Axis; bool Revert; float MuscleScale; }; const MuscleVisGroup gMuscleVisGroupsShort[MUSCLE_PATCH_SHORT_COUNT] = { { GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_HEAD, false, 1.0f }, { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_SPINE_MID, GESTURE_JOINT_SPINE_SHOULDER, false, 1.0f }, { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_HEAD, GESTURE_JOINT_SPINE_MID, false, 1.0f }, { GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_HAND_LEFT, GESTURE_JOINT_ELBOW_LEFT, true, 1.0f }, { GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_WRIST_LEFT, GESTURE_JOINT_SHOULDER_LEFT, true, 1.0f }, { GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_ELBOW_LEFT, GESTURE_JOINT_SPINE_SHOULDER, true, 1.0f }, { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_LEFT, GESTURE_JOINT_SPINE_MID, true, 1.0f }, { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_SPINE_MID , true, 2.0f }, { GESTURE_JOINT_HIP_LEFT, GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_SPINE_BASE , true, 2.0f }, { GESTURE_JOINT_KNEE_LEFT, GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_HIP_LEFT, true, 2.0f }, { GESTURE_JOINT_ANKLE_LEFT, GESTURE_JOINT_FOOT_LEFT, GESTURE_JOINT_KNEE_LEFT, true, 2.0f }, { GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_HAND_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, false, 1.0f }, { GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_WRIST_RIGHT, GESTURE_JOINT_SHOULDER_RIGHT, false, 1.0f }, { GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_ELBOW_RIGHT, GESTURE_JOINT_SPINE_SHOULDER, false, 1.0f }, { GESTURE_JOINT_SPINE_SHOULDER, GESTURE_JOINT_SHOULDER_RIGHT, GESTURE_JOINT_SPINE_MID, false, 1.0f }, { GESTURE_JOINT_SPINE_BASE, GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_SPINE_MID , true, 2.0f }, { GESTURE_JOINT_HIP_RIGHT, GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_SPINE_BASE, false, 2.0f }, { GESTURE_JOINT_KNEE_RIGHT, GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_HIP_RIGHT, false, 2.0f }, { GESTURE_JOINT_ANKLE_RIGHT, GESTURE_JOINT_FOOT_RIGHT, GESTURE_JOINT_KNEE_RIGHT, false, 2.0f }, }; bool FloatIsSane(float f) { return ((f > -1000000) && (f < 1000000)); } bool VectorIsSane(XMVECTOR v) { return true && FloatIsSane(XMVectorGetX(v)) && FloatIsSane(XMVectorGetY(v)) && FloatIsSane(XMVectorGetZ(v)); } enum TorqueSelector { TorqueSelect_AllDynamic = 0, TorqueSelect_StaticBalance, TorqueSelect_Motion, TorqueSelect_Balance, TorqueSelect_MuscleX, TorqueSelect_MuscleY, TorqueSelect_MuscleZ, }; XMVECTOR ValueFractions(XMVECTOR f) { XMFLOAT4 v; XMStoreFloat4(&v, XMVectorAbs(f)); float sum = v.x + v.y + v.z; if (sum == 0.0f) sum = 1.0f; return XMVectorSet( v.x / sum, v.y / sum, v.z / sum, 0.0f); } unsigned int MuscleFrameVisualize_Short( const MUSCLE_FRAME* pMuscles, TorqueSelector torqueSelector, MUSCLE_PATCH* pPositions) { const float invGoldenRatio = 0.5f / (1.0f + sqrtf(5.0f)); // width of unit arrow width MUSCLE_DATA zeroPos; #ifdef _XBOX XMemSet(&zeroPos, 0, sizeof(zeroPos)); #else memset(&zeroPos, 0, sizeof(zeroPos)); #endif if (pMuscles != NULL) { if (pMuscles->CenterOfMass.mLimbMassEstimate == 0) { // If muscles are not tracked, return all zeros #ifdef _XBOX XMemSet(pPositions, 0, sizeof(MUSCLE_PATCH) * MUSCLE_PATCH_SHORT_COUNT); #else memset(pPositions, 0, sizeof(MUSCLE_PATCH) * MUSCLE_PATCH_SHORT_COUNT); #endif return MUSCLE_PATCH_SHORT_COUNT; } // Now fill out the patches for (int vi = 0; vi < MUSCLE_PATCH_SHORT_COUNT; vi++) { const MuscleVisGroup& group = gMuscleVisGroupsShort[vi]; const MUSCLE_DATA* pInf = &(pMuscles->Muscles[group.Origin]); const MUSCLE_DATA* pEnd = &(pMuscles->Muscles[group.EndPoint]); XMVECTOR r = (pEnd->mJointPosition - pInf->mJointPosition); float sr = XMVectorGetX(XMVector3Length(r)) * invGoldenRatio * group.MuscleScale; // Skip this patch if it has no length, or if the joints have very low // confidense (i.e. very large resolution radius of 2 meters or more). if (sr == 0.0f || (pEnd->mJointResolutionRadius > 2.0f) || (pInf->mJointResolutionRadius > 2.0f) ) { #ifdef _XBOX XMemSet(&(pPositions[vi]), 0, sizeof(pPositions[vi])); #else memset(&(pPositions[vi]), 0, sizeof(pPositions[vi])); #endif continue; } // Calculate the basis forces: const MUSCLE_DATA *pCore = NULL, *pOther = NULL, *pForce = NULL, *pTorque = NULL; XMVECTOR aboutAxis, bendAxis, forceOffset; XMVECTOR torqueVec, forceVec; float magInf = 0, magEnd = 0; float torqueNormalizer = 1.0f; magInf = XMVectorGetX(XMVector3Length(pInf->mDynamicExternalForce)); magEnd = XMVectorGetX(XMVector3Length(pEnd->mDynamicExternalForce)); pForce = (magInf < magEnd) ? pEnd : pInf; float forceScaler = (sqrtf(gesture_max(magInf, magEnd) / pForce->mForceNormalizationConstant)); switch (torqueSelector) { case TorqueSelect_AllDynamic: pTorque = pEnd; torqueVec = pTorque->mDynamicLocalTorqueBalance + pTorque->mDynamicLocalTorqueMotion; forceVec = pForce->mDynamicExternalForce; torqueNormalizer = pTorque->mTorqueNormalizationConstant; break; case TorqueSelect_StaticBalance: magInf = XMVectorGetX(XMVector3Length(pInf->mStaticExternalForce)); magEnd = XMVectorGetX(XMVector3Length(pEnd->mStaticExternalForce)); pForce = (magInf < magEnd) ? pEnd : pInf; pTorque = pEnd; torqueVec = pTorque->mStaticLocalTorqueBalance; forceVec = pForce->mStaticExternalForce; torqueNormalizer = pTorque->mTorqueNormalizationConstant; forceScaler = (sqrtf(gesture_max(magInf, magEnd) / pForce->mForceNormalizationConstant)); break; case TorqueSelect_Balance: pTorque = pEnd; torqueVec = pTorque->mDynamicLocalTorqueBalance; forceVec = pForce->mDynamicExternalForce; torqueNormalizer = pTorque->mTorqueNormalizationConstant; break; case TorqueSelect_Motion: pTorque = pEnd; torqueVec = pTorque->mDynamicLocalTorqueMotion; forceVec = pForce->mDynamicExternalForce; torqueNormalizer = pTorque->mTorqueNormalizationConstant; break; case TorqueSelect_MuscleX: pTorque = pEnd; torqueVec = pTorque->mMuscleToSensorX * XMVectorGetX(pTorque->mMuscleDynamicLocalTorque); forceVec = pForce->mDynamicExternalForce; forceScaler *= XMVectorGetX(ValueFractions(pTorque->mMuscleDynamicLocalTorque)); torqueNormalizer = pTorque->mTorqueNormalizationConstant; break; case TorqueSelect_MuscleY: pTorque = pEnd; torqueVec = pTorque->mMuscleToSensorY * XMVectorGetY(pTorque->mMuscleDynamicLocalTorque); forceVec = pForce->mDynamicExternalForce; forceScaler *= XMVectorGetY(ValueFractions(pTorque->mMuscleDynamicLocalTorque));; torqueNormalizer = pTorque->mTorqueNormalizationConstant; break; case TorqueSelect_MuscleZ: pTorque = pEnd; torqueVec = pTorque->mMuscleToSensorZ * XMVectorGetZ(pTorque->mMuscleDynamicLocalTorque); forceVec = pForce->mDynamicExternalForce; forceScaler *= XMVectorGetZ(ValueFractions(pTorque->mMuscleDynamicLocalTorque));; torqueNormalizer = pTorque->mTorqueNormalizationConstant; break; default: torqueVec = XMVectorZero(); forceVec = XMVectorZero(); MUSCLE_TODO(); break; }; aboutAxis = (XMVector3Normalize(torqueVec) * (sr * forceScaler)); MUSCLE_ASSERT(VectorIsSane(aboutAxis)); bendAxis = (XMVector3Normalize(XMVector3Cross(r, aboutAxis)) * (sqrtf(fabsf(XMVectorGetX(XMVector3Length(torqueVec))) / torqueNormalizer)) * (-sr)); MUSCLE_ASSERT(VectorIsSane(bendAxis)); forceOffset = (forceVec * ((sr * -0.16f) / pForce->mForceNormalizationConstant)) - (bendAxis * 0.16f); MUSCLE_ASSERT(VectorIsSane(forceOffset)); pCore = (magInf > magEnd) ? pInf : pEnd; pOther = (magInf <= magEnd) ? pInf : pEnd; // Generate the patch representation: MUSCLE_PATCH patch; patch.BaseMid = pCore->mJointPosition - forceOffset; patch.BaseLeft = pCore->mJointPosition - aboutAxis + forceOffset; patch.BaseRight = pCore->mJointPosition + aboutAxis + forceOffset; patch.EndMid = pOther->mJointPosition; patch.EndLeft = pOther->mJointPosition; patch.EndRight = pOther->mJointPosition; patch.MidLeft = (patch.BaseLeft + patch.EndLeft) * 0.5f + bendAxis; //patch.MidMid = ( patch.BaseMid + patch.EndMid ) * 0.5f + bendAxis; patch.MidRight = (patch.BaseRight + patch.EndRight) * 0.5f + bendAxis; pPositions[vi] = patch; } } return MUSCLE_PATCH_SHORT_COUNT; } unsigned int MuscleFrameVisualize_Complete( const MUSCLE_FRAME* pMuscles, MUSCLE_PATCH* pPositions) { MuscleFrameVisualize_Short(pMuscles, TorqueSelect_MuscleX, pPositions); MuscleFrameVisualize_Short(pMuscles, TorqueSelect_MuscleY, pPositions + MUSCLE_PATCH_SHORT_COUNT); MuscleFrameVisualize_Short(pMuscles, TorqueSelect_MuscleZ, pPositions + (2 * MUSCLE_PATCH_SHORT_COUNT)); return MUSCLE_PATCH_COUNT; } unsigned int MuscleFrameVisualize( const MUSCLE_FRAME* pMuscles, int totalPatchCount, MUSCLE_PATCH* pOptPositions) { if (totalPatchCount == MUSCLE_PATCH_COUNT) { return MuscleFrameVisualize_Complete(pMuscles, pOptPositions); } else if (totalPatchCount == MUSCLE_PATCH_SHORT_COUNT) { return MuscleFrameVisualize_Short(pMuscles, TorqueSelect_AllDynamic, pOptPositions); } else if (totalPatchCount == 0) { return 0; } else { // TODO(); !!! Unknown number of patches to fill into!!! MUSCLE_TODO(); return 0; } } void MuscleFrameVisualizeAsFloats( const MUSCLE_FRAME* pMuscles, int totalPatchCount, float* pAsArrayOfPATCH_COUNTx8x3Floats) { MUSCLE_PATCH patches[MUSCLE_PATCH_COUNT]; MuscleFrameVisualize(pMuscles, totalPatchCount, patches); for (int pi = 0; piMEM = XMVectorLerp( pFrom->MEM, pTo->MEM, t ) #define INTERP1_MEM( MEM ) pInto->MEM = ( ( pFrom->MEM * ( 1.0f - t ) ) + ( pTo->MEM * t ) ) void MuscleDataInterpolate( MUSCLE_DATA* pInto, const MUSCLE_DATA* pFrom, const MUSCLE_DATA* pTo, float t) { INTERP3_MEM(mJointPosition); INTERP3_MEM(mLimbLinearVelocity); INTERP3_MEM(mLimbLinearAcceleration); INTERP3_MEM(mLimbAngularVelocity); INTERP3_MEM(mLimbAngularAcceleration); INTERP3_MEM(mLimbTempJerk); INTERP3_MEM(mLimbTempAcceleration); INTERP3_MEM(mStaticExternalForce); INTERP3_MEM(mStaticExternalTorque); INTERP3_MEM(mStaticLocalTorqueBalance); INTERP3_MEM(mDynamicExternalForce); INTERP3_MEM(mDynamicExternalTorque); INTERP3_MEM(mDynamicLocalTorqueMotion); INTERP3_MEM(mDynamicLocalTorqueBalance); INTERP3_MEM(mMuscleToSensorX); INTERP3_MEM(mMuscleToSensorY); INTERP3_MEM(mMuscleToSensorZ); INTERP3_MEM(mMuscleDynamicLocalTorque); INTERP1_MEM(mJointResolutionRadius); INTERP1_MEM(mLimbLength); INTERP1_MEM(mLimbMassEstimate); INTERP1_MEM(mLimbMassExternal); INTERP1_MEM(mLimbAngularExtension); INTERP1_MEM(mForceNormalizationConstant); INTERP1_MEM(mTorqueNormalizationConstant); } void MuscleFrameInterpolate( MUSCLE_FRAME* pInto, const MUSCLE_FRAME* pFrom, const MUSCLE_FRAME* pTo, float t) { for (int i = 0; i < GESTURE_JOINT_COUNT; i++) { MuscleDataInterpolate(pInto->Muscles + i, pFrom->Muscles + i, pTo->Muscles + i, t); } MuscleDataInterpolate(&pInto->CenterOfMass, &pFrom->CenterOfMass, &pTo->CenterOfMass, t); INTERP1_MEM(TimeSpan); } }