Example #1
0
void URMove::computeVelocities(){
    
    if( targetJointPose.size() >0) {
        if( currentJointVelocity.size() != currentPose.size() ) {
            currentJointVelocity.assign( currentPose.size(), 0.0f );
        }
        
        avgAccel        = 0;
        previousJointVelocity = currentJointVelocity;
        minSpeed = FLT_MAX;
        maxSpeed = FLT_MIN;
        for( int i = 0; i < currentPose.size(); i++ ) {
            currentJointVelocity[i] =  ofAngleDifferenceRadians( currentPose[i], targetPose[i] )/deltaTime/speedDivider;
            
            currentJointVelocity[i] = ofClamp(currentJointVelocity[i], -PI/2, PI/2);
            minSpeed = MIN(minSpeed.get(), currentJointVelocity[i]);
            maxSpeed = MAX(maxSpeed.get(), currentJointVelocity[i]);
            
            
            jointAccelerations[i] = (currentJointVelocity[i] - previousJointVelocity[i])/deltaTime/speedDivider;
            if(fabs(jointAccelerations[i]) > fabs(avgAccel)){
                avgAccel =jointAccelerations[i];
            }
            
        }
        
        //            cout << "avgAccel : " << avgAccel << endl;
        
    } else {
        //            for(int i = 0; i < currentJointVelocity.size(); i++){
        //                currentJointVelocity[i] = 0;
        //            }
    }
}
Example #2
0
//--------------------------------------------------
float ofLerpRadians(float currentAngle, float targetAngle, float pct) {
	return currentAngle + ofAngleDifferenceRadians(currentAngle,targetAngle) * pct;
}