//------------------------------------------------------------------------------ //! void adjustLimb( Skeleton::Instance& skelInst, const Puppeteer::PositionalConstraint& constraint, const Vec3f& dRoot ) { Skeleton* skeleton = skelInst.skeleton(); int cID = constraint.id(); const Skeleton::Limb& limb = skeleton->limbFromBone( cID ); int sID = limb.boneID(); int eID = limb.endEffectorID(); // 1. Compute mid angle. float b1Len = skeleton->bone( sID ).length(); float b2Len = skeleton->bone( eID ).length(); // Current angle. Vec3f cdir = constraint.currentPos()-skelInst.globalPosition(sID); float clenSqr = cdir.sqrLength(); float ccosAngle = (b1Len*b1Len + b2Len*b2Len - clenSqr) / (2*b1Len*b2Len); ccosAngle = CGM::clamp( ccosAngle, -1.0f, 1.0f ); float cangle = CGM::acos( ccosAngle ); // New angle. Vec3f dir = constraint.endPos()-(skelInst.globalPosition(sID)+dRoot); float lenSqr = dir.sqrLength(); float cosAngle = (b1Len*b1Len + b2Len*b2Len - lenSqr) / (2*b1Len*b2Len); cosAngle = CGM::clamp( cosAngle, -1.0f, 1.0f ); float angle = CGM::acos( cosAngle ); float diffAngle = cangle-angle; Quatf orient; switch( skeleton->bone( eID ).dof() ) { case Skeleton::RX: orient = Quatf::eulerX( diffAngle ); break; case Skeleton::RY: orient = Quatf::eulerY( diffAngle ); break; case Skeleton::RZ: orient = Quatf::eulerZ( diffAngle ); break; default: orient = Quatf::eulerX( diffAngle ); } orient = orient * skelInst.localOrientation( eID ); // 2. Compute start angle. // 2.1 target position in local space of first limb bone. Vec3f target = skelInst.globalReferential(sID).globalToLocal() * (constraint.endPos()-dRoot); // 2.2 Compute new current end effector position in limb space. Vec3f localPos = skelInst.localReferential(eID).position(); Vec3f curPos = Reff( orient, localPos ).toMatrix() * skeleton->bone(eID).endPoint(); // 2.3 Compute rotation quaternion. Quatf rot = Quatf::twoVecs( curPos, target ); rot = rot * skelInst.localOrientation(sID); // 3. Blend with slerp/nlerp. //skelInst.localOrientationNoUpdate( sID, skelInst.localOrientation(sID).slerp( rot, constraint.weight() ) ); //skelInst.localOrientationNoUpdate( eID, skelInst.localOrientation(eID).slerp( orient, constraint.weight() ) ); skelInst.localOrientationNoUpdate( sID, skelInst.localOrientation(sID).nlerp( rot, constraint.weight() ) ); skelInst.localOrientationNoUpdate( eID, skelInst.localOrientation(eID).nlerp( orient, constraint.weight() ) ); }
//------------------------------------------------------------------------------ //! void Puppeteer::resolveConstraints( Skeleton::Instance& skelInst, const Vector< PositionalConstraint >& constraints ) { if( constraints.empty() ) return; Skeleton* skeleton = skelInst.skeleton(); // Compute global root position. Vec3f root = skelInst.globalPosition(0); // 1. Transform constraints into end effector constraints. for( uint i = 0; i < constraints.size(); ++i ) { int cID = constraints[i].id(); Skeleton::Limb& limb = skeleton->limbFromBone( cID ); int eID = limb.endEffectorID(); Vec3f epos = skelInst.globalReferential( eID ).toMatrix() * skeleton->bone( eID ).endPoint(); constraints[i]._currentPos = epos; constraints[i]._endEffectorPos = constraints[i]._position; if( cID != eID ) { Vec3f cpos = skelInst.globalReferential( cID ).toMatrix() * skeleton->bone( cID ).endPoint(); constraints[i]._endEffectorPos -= cpos-epos; } Vec3f delta = root - skelInst.globalPosition( limb.boneID() ); constraints[i]._sphere.center( constraints[i]._endEffectorPos + delta ); constraints[i]._sphere.radius( limb.reachRadius()/constraints[i].weight() ); } // 2. Compute an approximate root position. Vec3f dRoot(0.0f); float weights = 0.0f; float maxWeight = 0.0f; for( uint i = 0; i < constraints.size(); ++i ) { dRoot += constraints[i].weight() * (constraints[i]._endEffectorPos-constraints[i]._currentPos); weights += constraints[i].weight(); if( constraints[i].weight() > maxWeight ) { maxWeight = constraints[i].weight(); } } dRoot *= (maxWeight/weights)*0.8f; root += dRoot; // 3. Adjust root position to satisfy all constraints. adjustRoot( constraints, root, dRoot ); // 4. Adjust limbs positions. for( uint i = 0; i < constraints.size(); ++i ) { adjustLimb( skelInst, constraints[i], dRoot ); } // 5. Adjust skeleton instance. skelInst.offset().position() += dRoot; skelInst.updateGlobalTransforms(); // TODO: Ajust end of limbs... }