Eigen::MatrixXd MinimumJerkTrajectory::getDesiredValues(double _time)
{
/**
* For details on the analytical point to point min-jerk formulation see:
* http://www.jneurosci.org/content/5/7/1688.full.pdf
* http://shadmehrlab.org/book/minimum_jerk/minimumjerk.htm
*/
if (startTrigger)
{
startTrigger = false;
t0 = _time;
}
Eigen::MatrixXd desiredValue = Eigen::MatrixXd::Zero(nDoF, TRAJ_DIM);
double tau = (_time - t0) / pointToPointDuration;
if ((tau <= TAU_MAX) && (currentWaypointIndex<(nWaypoints-1)))
{
if (nonRotationDof != 0)
{
Eigen::VectorXd alpha = waypoints.col(currentWaypointIndex+1) - waypoints.col(currentWaypointIndex);
desiredValue.block(0,POS_INDEX,nonRotationDof,1) = waypoints.col(currentWaypointIndex) + alpha * ( 10*pow(tau,3.0) - 15*pow(tau,4.0) + 6*pow(tau,5.0) );
desiredValue.block(0,VEL_INDEX,nonRotationDof,1) = Eigen::VectorXd::Zero(nDoF) + alpha * ( 30*pow(tau,2.0) - 60*pow(tau,3.0) + 30*pow(tau,4.0) );
desiredValue.block(0,ACC_INDEX,nonRotationDof,1) = Eigen::VectorXd::Zero(nDoF) + alpha * ( 60*pow(tau,1.0) - 180*pow(tau,2.0) + 120*pow(tau,3.0) );
}
if (endsWithQuaternion)
{
Eigen::Rotation3d qStart, qEnd;
eigenVectorToQuaternion(waypoints.block((nDoF-QUATERNION_DIM),(currentWaypointIndex), QUATERNION_DIM, 1), qStart);
eigenVectorToQuaternion(waypoints.block((nDoF-QUATERNION_DIM),(currentWaypointIndex+1), QUATERNION_DIM, 1), qEnd);
Eigen::Rotation3d interpolatedQuat = quaternionSlerp(tau, qStart, qEnd);
Eigen::VectorXd interpolatedQuatVector = quaternionToEigenVector(interpolatedQuat);
desiredValue.block((nDoF-QUATERNION_DIM),POS_INDEX,QUATERNION_DIM,1) = interpolatedQuatVector;
}
}
else if ((tau > TAU_MAX) && (currentWaypointIndex<(nWaypoints-1)))
{
startTrigger = true;
currentWaypointIndex++;
desiredValue.col(POS_INDEX) = waypoints.col(currentWaypointIndex);
if (currentWaypointIndex<nWaypoints-1) {
pointToPointDuration = pointToPointDurationVector(currentWaypointIndex);
}
}
else{
desiredValue.col(POS_INDEX) = waypoints.col(nWaypoints-1);
trajectoryFinished = true;
}
return desiredValue;
}
static void interpolateJob( void* pData )
{
tKeyFrameJobData* pJobData = (tKeyFrameJobData *)pData;
Vector4Lerp( pJobData->mpResultPos,
pJobData->mpPos1,
pJobData->mpPos0,
pJobData->mfPct );
Vector4Lerp( pJobData->mpResultScale,
pJobData->mpScale0,
pJobData->mpScale1,
pJobData->mfPct );
quaternionSlerp( pJobData->mpResultRot,
pJobData->mpRot0,
pJobData->mpRot1,
pJobData->mfPct );
Vector4Lerp( pJobData->mpResultRotVec,
pJobData->mpRotVec0,
pJobData->mpRotVec1,
pJobData->mfPct );
}
void animSequencePlay( tAnimSequence const* pAnimSequence,
tAnimHierarchy const* pAnimHierarchy,
tVector4* aPositions,
tVector4* aScalings,
tQuaternion* aRotations,
tVector4* aRotationVecs,
tMatrix44* aAnimMatrices,
float fTime )
{
float fNumLoops = floorf( fTime / pAnimSequence->mfLastTime );
float fFrameTime = fTime - fNumLoops * pAnimSequence->mfLastTime;
// key frame for the joints
int iNumJoints = pAnimSequence->miNumJoints;
for( int i = 0; i < iNumJoints; i++ )
{
int iStart = pAnimSequence->maiStartFrames[i];
int iEnd = pAnimSequence->maiEndFrames[i];
// no valid animation frame for this joint
if( iStart < 0 || iEnd < 0 )
{
continue;
}
// find the ending frame
int iEndFrame = iStart;
for( iEndFrame = iStart; iEndFrame <= iEnd; iEndFrame++ )
{
if( pAnimSequence->mafTime[iEndFrame] > fFrameTime )
{
break;
}
} // for end frame = start to end
// didn't find end frame
if( iEndFrame > iEnd )
{
iEndFrame = iEnd;
}
int iStartFrame = iEndFrame - 1;
if( iStartFrame < 0 )
{
iStartFrame = 0;
}
// just at the start
if( iStartFrame < iStart )
{
iStartFrame = iStart;
}
WTFASSERT2( iStartFrame >= iStart && iStartFrame <= iEnd, "out of bounds looking for start animation frame" );
WTFASSERT2( iEndFrame >= iStart && iEndFrame <= iEnd, "out of bounds looking for end animation frame" );
float fTimeDuration = pAnimSequence->mafTime[iEndFrame] - pAnimSequence->mafTime[iStartFrame];
float fTimeFromStart = fFrameTime - pAnimSequence->mafTime[iStartFrame];
float fPct = 0.0f;
if( fTimeDuration > 0.0f )
{
fPct = fTimeFromStart / fTimeDuration;
}
// clamp pct
if( fPct > 1.0f )
{
fPct = 1.0f;
}
Vector4Lerp( &aPositions[i],
&pAnimSequence->maPositions[iStartFrame],
&pAnimSequence->maPositions[iEndFrame],
fPct );
Vector4Lerp( &aScalings[i],
&pAnimSequence->maScalings[iStartFrame],
&pAnimSequence->maScalings[iEndFrame],
fPct );
quaternionSlerp( &aRotations[i],
&pAnimSequence->maRotation[iStartFrame],
&pAnimSequence->maRotation[iEndFrame],
fPct );
Vector4Lerp( &aRotationVecs[i],
&pAnimSequence->maRotationVec[iStartFrame],
&pAnimSequence->maRotationVec[iEndFrame],
fPct );
#if 0
tJoint* pJoint = pAnimSequence->mapJoints[iStart];
tJob job;
tKeyFrameJobData interpJobData;
interpJobData.mfPct = fPct;
interpJobData.mpPos0 = &pAnimSequence->maPositions[iStartFrame];
interpJobData.mpPos1 = &pAnimSequence->maPositions[iEndFrame];
interpJobData.mpScale0 = &pAnimSequence->maScalings[iStartFrame];
interpJobData.mpScale1 = &pAnimSequence->maScalings[iEndFrame];
interpJobData.mpRot0 = &pAnimSequence->maRotation[iStartFrame];
interpJobData.mpRot1 = &pAnimSequence->maRotation[iEndFrame];
interpJobData.mpResultPos = &aPositions[i];
interpJobData.mpResultScale = &aScalings[i];
interpJobData.mpResultRot = &aRotations[i];
interpJobData.mpJoint = pJoint;
interpJobData.mpRotVec0 = &pAnimSequence->maRotationVec[iStartFrame];
interpJobData.mpRotVec1 = &pAnimSequence->maRotationVec[iEndFrame];
interpJobData.mpResultRotVec = &aRotationVecs[i];
job.mpfnFunc = interpolateJob;
job.mpData = &interpJobData;
job.miDataSize = sizeof( tKeyFrameJobData );
jobManagerAddJob( gpJobManager, &job );
#endif // #if 0
}
#if 0
tVector4* pScale = &aScalings[i];
tVector4* pPosition = &aPositions[i];
// interpolate position, scale, and rotation
Vector4Lerp( pPosition,
&pAnimSequence->maPositions[iEndFrame],
&pAnimSequence->maPositions[iStartFrame],
fPct );
Vector4Lerp( pScale,
&pAnimSequence->maScalings[iEndFrame],
&pAnimSequence->maScalings[iStartFrame],
fPct );
quaternionSlerp( &aRotations[i],
&pAnimSequence->maRotation[iEndFrame],
&pAnimSequence->maRotation[iStartFrame],
fPct );
jobManagerWait( gpJobManager );
#endif // #if 0
for( int i = 0; i < iNumJoints; i++ )
{
int iStart = pAnimSequence->maiStartFrames[i];
int iEnd = pAnimSequence->maiEndFrames[i];
// no valid animation frame for this joint
if( iStart < 0 || iEnd < 0 )
{
continue;
}
#if 0
tJob job;
tMatrixJobData matrixJobData;
matrixJobData.mpPosition = &aPositions[i];
matrixJobData.mpRotation = &aRotations[i];
matrixJobData.mpScale = &aScalings[i];
matrixJobData.mpResultMat = &aAnimMatrices[i];
matrixJobData.mpJoint = pAnimSequence->mapUniqueJoints[i];
matrixJobData.mpRotationVec = &aRotationVecs[i];
job.mpfnFunc = &matrixUpdateJob;
job.mpData = &matrixJobData;
job.miDataSize = sizeof( tMatrixJobData );
jobManagerAddJob( gpJobManager, &job );
#endif // #if 0
tMatrix44 posMatrix, scaleMatrix, rotMatrix, posRotMatrix;
//quaternionToMatrix( &rotMatrix, pMatrixData->mpRotation );
tMatrix44 rotMatZY, rotMatX, rotMatY, rotMatZ;
Matrix44RotateX( &rotMatX, aRotationVecs[i].fX );
Matrix44RotateY( &rotMatY, aRotationVecs[i].fY );
Matrix44RotateZ( &rotMatZ, aRotationVecs[i].fZ );
Matrix44Multiply( &rotMatZY, &rotMatZ, &rotMatY );
Matrix44Multiply( &rotMatrix, &rotMatZY, &rotMatX );
Matrix44Scale( &scaleMatrix, aScalings[i].fX, aScalings[i].fY, aScalings[i].fZ );
Matrix44Translate( &posMatrix, &aPositions[i] );
Matrix44Multiply( &posRotMatrix, &posMatrix, &rotMatrix );
Matrix44Multiply( &aAnimMatrices[i], &posRotMatrix, &scaleMatrix );
#if 0
// concat for animation matrix
tMatrix44 posMatrix, scaleMatrix, rotMatrix, posRotMatrix;
quaternionToMatrix( &rotMatrix, &aRotations[i] );
Matrix44Scale( &scaleMatrix, pScale->fX, pScale->fY, pScale->fZ );
Matrix44Translate( &posMatrix, pPosition );
Matrix44Multiply( &posRotMatrix, &posMatrix, &rotMatrix );
tMatrix44* pAnimMatrix = &aAnimMatrices[i];
Matrix44Multiply( pAnimMatrix, &posRotMatrix, &scaleMatrix );
#endif // #if 0
} // for i = 0 to num joints
jobManagerWait( gpJobManager );
}
void animSequenceUpdateJointAnimValues( tAnimSequence const* pAnimSequence,
tAnimHierarchy const* pAnimHierarchy,
tVector4* aPositions,
tVector4* aScalings,
tQuaternion* aRotations,
tVector4* aRotationVecs,
float fTime )
{
float fNumLoops = 0.0f;
//if( pAnimSequence->mfLastTime > 0.0f )
//{
// fNumLoops = floorf( fTime / pAnimSequence->mfLastTime );
//}
float fFrameTime = fTime - fNumLoops * pAnimSequence->mfLastTime;
// key frame for the joints
int iNumJoints = pAnimSequence->miNumJoints;
for( int i = 0; i < iNumJoints; i++ )
{
int iStart = pAnimSequence->maiStartFrames[i];
int iEnd = pAnimSequence->maiEndFrames[i];
// no valid animation frame for this joint
if( iStart < 0 || iEnd < 0 )
{
continue;
}
tJoint* pJoint = pAnimSequence->mapJoints[iStart];
// find the ending frame
int iEndFrame = iStart;
for( iEndFrame = iStart; iEndFrame <= iEnd; iEndFrame++ )
{
if( pAnimSequence->mafTime[iEndFrame] > fFrameTime )
{
break;
}
} // for end frame = start to end
// didn't find end frame
if( iEndFrame > iEnd )
{
iEndFrame = iEnd;
}
int iStartFrame = iEndFrame - 1;
if( iStartFrame < 0 )
{
iStartFrame = 0;
}
// just at the start
if( iStartFrame < iStart )
{
iStartFrame = iStart;
}
WTFASSERT2( iStartFrame >= iStart && iStartFrame <= iEnd, "out of bounds looking for start animation frame" );
WTFASSERT2( iEndFrame >= iStart && iEndFrame <= iEnd, "out of bounds looking for end animation frame" );
float fTimeDuration = pAnimSequence->mafTime[iEndFrame] - pAnimSequence->mafTime[iStartFrame];
float fTimeFromStart = fFrameTime - pAnimSequence->mafTime[iStartFrame];
float fPct = 0.0f;
if( fTimeDuration > 0.0f )
{
fPct = fTimeFromStart / fTimeDuration;
}
// clamp pct
if( fPct > 1.0f )
{
fPct = 1.0f;
}
int iJointIndex = 0;
int iNumHierarchyJoints = pAnimHierarchy->miNumJoints;
for( iJointIndex = 0; iJointIndex < iNumHierarchyJoints; iJointIndex++ )
{
if( !strcmp( pJoint->mszName, pAnimHierarchy->maJoints[iJointIndex].mszName ) )
{
break;
}
}
WTFASSERT2( iJointIndex == i, "TEST" );
WTFASSERT2( iJointIndex < iNumHierarchyJoints, "can't find joint in hierarchy" );
Vector4Lerp( &aPositions[i],
&pAnimSequence->maPositions[iStartFrame],
&pAnimSequence->maPositions[iEndFrame],
fPct );
Vector4Lerp( &aScalings[i],
&pAnimSequence->maScalings[iStartFrame],
&pAnimSequence->maScalings[iEndFrame],
fPct );
quaternionSlerp( &aRotations[i],
&pAnimSequence->maRotation[iStartFrame],
&pAnimSequence->maRotation[iEndFrame],
fPct );
Vector4Lerp( &aRotationVecs[i],
&pAnimSequence->maRotationVec[iStartFrame],
&pAnimSequence->maRotationVec[iEndFrame],
fPct );
#if 0
tJob job;
tKeyFrameJobData interpJobData;
interpJobData.mfPct = fPct;
interpJobData.mpPos0 = &pAnimSequence->maPositions[iStartFrame];
interpJobData.mpPos1 = &pAnimSequence->maPositions[iEndFrame];
interpJobData.mpScale0 = &pAnimSequence->maScalings[iStartFrame];
interpJobData.mpScale1 = &pAnimSequence->maScalings[iEndFrame];
interpJobData.mpRot0 = &pAnimSequence->maRotation[iStartFrame];
interpJobData.mpRot1 = &pAnimSequence->maRotation[iEndFrame];
interpJobData.mpResultPos = &aPositions[iJointIndex];
interpJobData.mpResultScale = &aScalings[iJointIndex];
interpJobData.mpResultRot = &aRotations[iJointIndex];
interpJobData.mpJoint = pJoint;
interpJobData.mpRotVec0 = &pAnimSequence->maRotationVec[iStartFrame];
interpJobData.mpRotVec1 = &pAnimSequence->maRotationVec[iEndFrame];
interpJobData.mpResultRotVec = &aRotationVecs[iJointIndex];
job.mpfnFunc = interpolateJob;
job.mpData = &interpJobData;
job.miDataSize = sizeof( tKeyFrameJobData );
jobManagerAddJob( gpJobManager, &job );
#endif // #if 0
} // for i = 0 to num joints
}
