CVector3 InterpolateVector(CVector3 vA, CVector3 vB, double blend) { if (blend == 0) return vA; else if (blend == 1) return vB; CVector3 cross; cross.Cross(vA, vB); cross.NormalizeInPlace(); double lengthA = vA.GetLength(); double lengthB = vB.GetLength(); double length = lengthB * blend + lengthA * (1 - blend); double a = vA.GetAngle(vB); CVector3 v = rotateVectorAlongAxis(vA, cross, a * blend); v.NormalizeInPlace(); v.ScaleInPlace(length); return v; }
/////////////////////////////////////////////////////////////// // METHODS /////////////////////////////////////////////////////////////// // GetIKTransform ============================================= CTransformation GetIKTransform(s_GetIKTransform values, CString outportName) { // prepare all variables CTransformation result; CVector3 bonePos,rootPos,effPos,upvPos,rootEff, xAxis,yAxis,zAxis, rollAxis; rootPos = values.root.GetTranslation(); effPos = values.eff.GetTranslation(); upvPos = values.upv.GetTranslation(); rootEff.Sub(effPos,rootPos); rollAxis.Normalize(rootEff); CMatrix4 rootMatrix = values.root.GetMatrix4(); CMatrix4 rootMatrixNeg; rootMatrixNeg.Invert(rootMatrix); double rootEffDistance = rootEff.GetLength(); // init the scaling double global_scale = values.root.GetScaling().GetX(); result.SetScaling(values.root.GetScaling()); // Distance with MaxStretch --------------------- double restLength = values.lengthA * values.scaleA + values.lengthB * values.scaleB; CVector3 distanceVector; distanceVector.MulByMatrix4(effPos, rootMatrixNeg); double distance = distanceVector.GetLength(); double distance2 = distance; if (distance > (restLength * (values.maxstretch))) { distanceVector.NormalizeInPlace(); distanceVector.ScaleInPlace(restLength * (values.maxstretch) ); distance = restLength * (values.maxstretch); } // Adapt Softness value to chain length -------- values.softness = values.softness * restLength *.1; // Stretch and softness ------------------------ // We use the real distance from root to controler to calculate the softness // This way we have softness working even when there is no stretch double stretch = max(1, distance / restLength); double da = restLength - values.softness; if (values.softness > 0 && distance2 > da) { double newlen = values.softness*(1.0 - exp(-(distance2 -da)/values.softness)) + da; stretch = distance / newlen; } values.lengthA = values.lengthA * stretch * values.scaleA * global_scale; values.lengthB = values.lengthB * stretch * values.scaleB * global_scale; // Reverse ------------------------------------- double d = distance / (values.lengthA + values.lengthB); double reverse_scale; if (values.reverse < 0.5) reverse_scale = 1-(values.reverse*2 * (1-d)); else reverse_scale = 1-((1-values.reverse)*2 * (1-d)); values.lengthA *= reverse_scale; values.lengthB *= reverse_scale; bool invert = values.reverse > 0.5; // Slide --------------------------------------- double slide_add; if (values.slide < .5) slide_add = (values.lengthA * (values.slide * 2)) - (values.lengthA); else slide_add = (values.lengthB * (values.slide * 2)) - (values.lengthB); values.lengthA += slide_add; values.lengthB -= slide_add; // calculate the angle inside the triangle! double angleA = 0; double angleB = 0; // check if the divider is not null otherwise the result is nan // and the output disapear from xsi, that breaks constraints if((rootEffDistance < values.lengthA + values.lengthB) && (rootEffDistance > fabs(values.lengthA - values.lengthB) + 1E-6)) { // use the law of cosine for lengthA double a = values.lengthA; double b = rootEffDistance; double c = values.lengthB; angleA = acos(min(1, (a * a + b * b - c * c ) / ( 2 * a * b))); // use the law of cosine for lengthB a = values.lengthB; b = values.lengthA; c = rootEffDistance; angleB = acos(min(1, (a * a + b * b - c * c ) / ( 2 * a * b))); // invert the angles if need be if(invert) { angleA = -angleA; angleB = -angleB; } } // start with the X and Z axis xAxis = rootEff; yAxis.LinearlyInterpolate(rootPos,effPos,.5); yAxis.Sub(upvPos,yAxis); yAxis = rotateVectorAlongAxis(yAxis, rollAxis, values.roll); zAxis.Cross(xAxis,yAxis); xAxis.NormalizeInPlace(); zAxis.NormalizeInPlace(); // switch depending on our mode if(outportName == "OutBoneA") // Bone A { // check if we need to rotate the bone if(angleA != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleA); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } if (values.negate) xAxis.NegateInPlace(); // cross the yAxis and normalize yAxis.Cross(zAxis,xAxis); yAxis.NormalizeInPlace(); // output the rotation result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis); // set the scaling + the position result.SetSclX(values.lengthA); result.SetTranslation(rootPos); } else if(outportName == "OutBoneB") // Bone B { // check if we need to rotate the bone if(angleA != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleA); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } // calculate the position of the elbow! bonePos.Scale(values.lengthA,xAxis); bonePos.AddInPlace(rootPos); // check if we need to rotate the bone if(angleB != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleB-XSI::MATH::PI); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } if (values.negate) xAxis.NegateInPlace(); // cross the yAxis and normalize yAxis.Cross(zAxis,xAxis); yAxis.NormalizeInPlace(); // output the rotation result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis); // set the scaling + the position result.SetSclX(values.lengthB); result.SetTranslation(bonePos); } else if(outportName == "OutCenter") // center { // check if we need to rotate the bone bonePos.Scale(values.lengthA,xAxis); if(angleA != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleA); bonePos.MulByMatrix3InPlace(rot.GetMatrix()); } bonePos.AddInPlace(rootPos); // cross the yAxis and normalize yAxis.Sub(upvPos,bonePos); zAxis.Cross(xAxis,yAxis); zAxis.NormalizeInPlace(); if (values.negate) xAxis.NegateInPlace(); yAxis.Cross(zAxis,xAxis); yAxis.NormalizeInPlace(); // output the rotation result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis); // set the scaling + the position result.SetSclX(stretch * values.root.GetSclX()); result.SetTranslation(bonePos); } else if(outportName == "OutCenterN") // center normalized { // check if we need to rotate the bone if(angleA != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleA); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } // calculate the position of the elbow! bonePos.Scale(values.lengthA,xAxis); bonePos.AddInPlace(rootPos); // check if we need to rotate the bone if(angleB != 0.0) { if(invert) angleB += XSI::MATH::PI * 2; CRotation rot; rot.SetFromAxisAngle(zAxis,angleB*.5-XSI::MATH::PI*.5); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } // cross the yAxis and normalize // yAxis.Sub(upvPos,bonePos); // this was flipping the centerN when the elbow/upv was aligned to root/eff zAxis.Cross(xAxis,yAxis); zAxis.NormalizeInPlace(); if (values.negate) xAxis.NegateInPlace(); yAxis.Cross(zAxis,xAxis); yAxis.NormalizeInPlace(); // output the rotation result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis); // set the scaling + the position // result.SetSclX(stretch * values.root.GetSclX()); result.SetTranslation(bonePos); } else if(outportName == "OutEff") // effector { // check if we need to rotate the bone effPos = rootPos; if(angleA != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleA); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } // calculate the position of the elbow! bonePos.Scale(values.lengthA,xAxis); effPos.AddInPlace(bonePos); // check if we need to rotate the bone if(angleB != 0.0) { CRotation rot; rot.SetFromAxisAngle(zAxis,angleB-XSI::MATH::PI); xAxis.MulByMatrix3InPlace(rot.GetMatrix()); } // calculate the position of the effector! bonePos.Scale(values.lengthB,xAxis); effPos.AddInPlace(bonePos); // cross the yAxis and normalize yAxis.Cross(zAxis,xAxis); yAxis.NormalizeInPlace(); // output the rotation result = values.eff; result.SetTranslation(effPos); } CRotation r = result.GetRotation(); CVector3 eulerAngles = r.GetXYZAngles(); double rx = eulerAngles.GetX(); double ry = eulerAngles.GetY(); double rz = eulerAngles.GetZ(); return result; }
// Update ================================================================================= CStatus gStretchOp2_Update( CRef& in_ctxt ) { OperatorContext ctxt( in_ctxt ); // User Datas ------------------------------------ CValue::siPtrType pUserData = ctxt.GetUserData(); OpUserData* pOpState = (OpUserData*)pUserData; if ( pOpState == NULL || pOpState->index >= 4) { // First time called pOpState = new OpUserData(); ctxt.PutUserData( (CValue::siPtrType)pOpState ); // Inputs --------------------------------------- KinematicState kRoot(ctxt.GetInputValue(0)); KinematicState kCtrl(ctxt.GetInputValue(1)); CTransformation tRoot(kRoot.GetTransform()); CTransformation tCtrl(kCtrl.GetTransform()); CVector3 vRoot = tRoot.GetTranslation(); CVector3 vCtrl = tCtrl.GetTranslation(); CMatrix4 mRoot = tRoot.GetMatrix4(); CMatrix4 mRootNeg; mRootNeg.Invert(mRoot); double dRest0 = ctxt.GetParameterValue(L"rest0"); double dRest1 = ctxt.GetParameterValue(L"rest1"); double dPrefRot = ctxt.GetParameterValue(L"prefrot"); double dScale0 = ctxt.GetParameterValue(L"scale0"); double dScale1 = ctxt.GetParameterValue(L"scale1"); double dSoftness = ctxt.GetParameterValue(L"soft"); double dMaxStretch = ctxt.GetParameterValue(L"maxstretch"); double dSlide = ctxt.GetParameterValue(L"slide"); double dReverse = ctxt.GetParameterValue(L"reverse"); // Distance with MaxStretch --------------------- double dRestLength = dRest0 * dScale0 + dRest1 * dScale1; CVector3 vDistance; vDistance.MulByMatrix4(vCtrl, mRootNeg); double dDistance = vDistance.GetLength(); double dDistance2 = dDistance; if (dDistance > (dRestLength * dMaxStretch)) { vDistance.NormalizeInPlace(); vDistance.ScaleInPlace(dRestLength * dMaxStretch); dDistance = dRestLength * dMaxStretch; } // Adapt Softness value to chain length -------- dSoftness *= dRestLength*.1; // Stretch and softness ------------------------ /// We use the real distance from root to controler to calculate the softness /// This way we have softness working even when there is no stretch double dStretch = dDistance/dRestLength; if (dStretch < 1) dStretch = 1; double da = dRestLength - dSoftness; if (dSoftness > 0 && dDistance2 > da) { double newlen = dSoftness*(1.0 - exp(-(dDistance2 -da)/dSoftness)) + da; dStretch = dDistance / newlen; } double dLength0 = dRest0 * dStretch * dScale0; double dLength1 = dRest1 * dStretch * dScale1; // Reverse ------------------------------------- double d = dDistance/(dLength0 + dLength1); double dScale; if (dReverse < 0.5) dScale = 1-(dReverse*2 * (1-d)); else dScale = 1-((1-dReverse)*2 * (1-d)); dLength0 *= dScale; dLength1 *= dScale; dPrefRot = -(dReverse-0.5) * 2 * dPrefRot; // Slide --------------------------------------- double dAdd; if (dSlide < .5) dAdd = (dLength0 * (dSlide*2)) - (dLength0); else dAdd = (dLength1 * (dSlide*2)) - (dLength1); dLength0 += dAdd; dLength1 -= dAdd; // Effector Position ---------------------------- CTransformation t; vDistance.MulByMatrix4(vDistance, mRoot); t.SetTranslation(vDistance); pOpState->index = 0; pOpState->t = t; pOpState->dLength0 = dLength0; pOpState->dLength1 = dLength1; pOpState->dPrefRot = dPrefRot; } // Outputs ------------------------------------- CRef outputPortRef=ctxt.GetOutputPort(); OutputPort OutPort(outputPortRef); // Effector Transform if (OutPort.GetIndex() == 2) { KinematicState kOut = ctxt.GetOutputTarget(); kOut.PutTransform(pOpState->t); } // Bone 0 Length else if (OutPort.GetIndex() == 3) { OutPort.PutValue(pOpState->dLength0); } // Bone 1 Length else if (OutPort.GetIndex() == 4) { OutPort.PutValue(pOpState->dLength1); } // Bone 1 PrefRot else if (OutPort.GetIndex() == 5) { OutPort.PutValue(pOpState->dPrefRot); } pOpState->index += 1; return CStatus::OK; }
// Update ============================================================================= CStatus gStretchOp2Multi_Update( CRef& in_ctxt ) { OperatorContext ctxt( in_ctxt ); // User Datas ------------------------------------ CValue::siPtrType pUserData = ctxt.GetUserData(); OpUserData* pOpState = (OpUserData*)pUserData; if ( pOpState == NULL || pOpState->index >= 2) { // First time called pOpState = new OpUserData(); ctxt.PutUserData( (CValue::siPtrType)pOpState ); // Inputs --------------------------------------- KinematicState kRoot(ctxt.GetInputValue(0)); KinematicState kCtrl(ctxt.GetInputValue(1)); CTransformation tRoot(kRoot.GetTransform()); CTransformation tCtrl(kCtrl.GetTransform()); CVector3 vRoot = tRoot.GetTranslation(); CVector3 vCtrl = tCtrl.GetTranslation(); CMatrix4 mRoot = tRoot.GetMatrix4(); CMatrix4 mRootNeg; mRootNeg.Invert(mRoot); double dRestLength = ctxt.GetParameterValue(L"restlength"); double dScale = ctxt.GetParameterValue(L"scale"); double dSoftness = ctxt.GetParameterValue(L"soft"); double dMaxStretch = ctxt.GetParameterValue(L"maxstretch"); // Distance with MaxStretch --------------------- dRestLength = dRestLength * dScale - .00001; CVector3 vDistance; vDistance.MulByMatrix4(vCtrl, mRootNeg); double dDistance = vDistance.GetLength(); double dDistance2 = dDistance; if (dDistance > (dRestLength * dMaxStretch)) { vDistance.NormalizeInPlace(); vDistance.ScaleInPlace(dRestLength * dMaxStretch); dDistance = dRestLength * dMaxStretch; } Application app; app.LogMessage(L"dist : "+CString(dDistance)); app.LogMessage(L"dist2 : "+CString(dDistance2)); // Adapt Softness value to chain length -------- dSoftness = dSoftness * dRestLength *.1; // Stretch and softness ------------------------ /// We use the real distance from root to controler to calculate the softness /// This way we have softness working even when there is no stretch double dStretch = dDistance/dRestLength; if (dStretch < 1) dStretch = 1; double da = dRestLength - dSoftness; if (dSoftness > 0 && dDistance2 > da) { double newlen = dSoftness*(1.0 - exp(-(dDistance2 -da)/dSoftness)) + da; dStretch = dDistance / newlen; } double dScaleX = dStretch * dScale; app.LogMessage(L"scalex : "+CString(dScaleX)); // Effector Position ---------------------------- CTransformation t; vDistance.MulByMatrix4(vDistance, mRoot); t.SetTranslation(vDistance); pOpState->index = 0; pOpState->t = t; pOpState->dLength0 = dScaleX; } // Outputs ------------------------------------- CRef outputPortRef=ctxt.GetOutputPort(); OutputPort OutPort(outputPortRef); // Effector Transform if (OutPort.GetIndex() == 2) { KinematicState kOut = ctxt.GetOutputTarget(); kOut.PutTransform(pOpState->t); } // Bone 0 Length else if (OutPort.GetIndex() == 3) { OutPort.PutValue(pOpState->dLength0); } pOpState->index += 1; return CStatus::OK; }
/////////////////////////////////////////////////////////////// // METHOD /////////////////////////////////////////////////////////////// CStatus splinekine_op_Update( CRef& in_ctxt, int mode = 0 ) { OperatorContext ctxt( in_ctxt ); long count = (LONG)ctxt.GetParameterValue(L"count"); CVector3Array pos(count); CVector3Array tangent(count); CVector3Array rot(count); CDoubleArray roll(count); CVector3Array rotParent(count); CVector3Array scl(count); CTransformation singleXF,singleParentXF; CMatrix3 singleMat; CTransformation modelXF; if(mode == 0) // simple 180 flipping interpolation { for(long i=0;i<count;i++) { singleXF = KinematicState(ctxt.GetInputValue(i)).GetTransform(); singleMat = singleXF.GetRotationMatrix3(); pos[i] = singleXF.GetTranslation(); tangent[i].MulByMatrix3(CVector3(singleXF.GetSclX()*2.5,0,0),singleMat); rot[i] = singleXF.GetRotationXYZAngles(); scl[i] = singleXF.GetScaling(); } modelXF = KinematicState(ctxt.GetInputValue(count)).GetTransform(); } else if(mode == 1) // local rotation X based roll { for(long i=0;i<count;i++) { singleParentXF = KinematicState(ctxt.GetInputValue(i*2+0)).GetTransform(); singleXF = KinematicState(ctxt.GetInputValue(i*2+1)).GetTransform(); roll[i] = singleXF.GetRotX(); singleXF = MapObjectPoseToWorldSpace(singleParentXF,singleXF); singleMat = singleXF.GetRotationMatrix3(); pos[i] = singleXF.GetTranslation(); tangent[i].MulByMatrix3(CVector3(singleXF.GetSclX()*2.5,0,0),singleMat); rot[i] = singleParentXF.GetRotationXYZAngles(); scl[i] = singleXF.GetScaling(); } modelXF = KinematicState(ctxt.GetInputValue(count*2)).GetTransform(); } double u = ctxt.GetParameterValue(L"u"); bool resample = ctxt.GetParameterValue(L"resample"); long subdiv = (LONG)ctxt.GetParameterValue(L"subdiv"); bool absolute = ctxt.GetParameterValue(L"absolute"); double step = 1.0 / max(1,count-1); int index1 = (int)min(count-2,u / step); int index2 = index1+1; int index1temp = index1; int index2temp = index2; double v = (u - step * double(index1)) / step; double vtemp = v; // calculate the bezier CVector3 bezierPos; CVector3 xAxis,yAxis,zAxis; if(!resample) { // straight bezier solve CVector3Array results = bezier4point(pos[index1],tangent[index1],pos[index2],tangent[index2],v); bezierPos = results[0]; xAxis = results[1]; } else if(!absolute) { CVector3Array presample(subdiv); CVector3Array presampletan(subdiv); CDoubleArray samplelen(subdiv); double samplestep = 1.0 / double(subdiv-1); double sampleu = samplestep; presample[0] = pos[index1]; presampletan[0] = tangent[index1]; CVector3 prevsample(presample[0]); CVector3 diff; samplelen[0] = 0; double overalllen = 0; CVector3Array results(2); for(long i=1;i<subdiv;i++,sampleu+=samplestep) { results = bezier4point(pos[index1],tangent[index1],pos[index2],tangent[index2],sampleu); presample[i] = results[0]; presampletan[i] = results[1]; diff.Sub(presample[i],prevsample); overalllen += diff.GetLength(); samplelen[i] = overalllen; prevsample = presample[i]; } // now as we have the sampleu = 0; for(long i=0;i<subdiv-1;i++,sampleu+=samplestep) { samplelen[i+1] = samplelen[i+1] / overalllen; if(v>=samplelen[i] && v <= samplelen[i+1]) { v = (v - samplelen[i]) / (samplelen[i+1] - samplelen[i]); bezierPos.LinearlyInterpolate(presample[i],presample[i+1],v); xAxis.LinearlyInterpolate(presampletan[i],presampletan[i+1],v); break; } } } else { CVector3Array presample(subdiv); CVector3Array presampletan(subdiv); CDoubleArray samplelen(subdiv); double samplestep = 1.0 / double(subdiv-1); double sampleu = samplestep; presample[0] = pos[0]; presampletan[0] = tangent[0]; CVector3 prevsample(presample[0]); CVector3 diff; samplelen[0] = 0; double overalllen = 0; CVector3Array results; for(long i=1;i<subdiv;i++,sampleu+=samplestep) { index1 = (int)min(count-2,sampleu / step); index2 = index1+1; v = (sampleu - step * double(index1)) / step; results = bezier4point(pos[index1],tangent[index1],pos[index2],tangent[index2],v); presample[i] = results[0]; presampletan[i] = results[1]; diff.Sub(presample[i],prevsample); overalllen += diff.GetLength(); samplelen[i] = overalllen; prevsample = presample[i]; } // now as we have the sampleu = 0; for(long i=0;i<subdiv-1;i++,sampleu+=samplestep) { samplelen[i+1] = samplelen[i+1] / overalllen; if(u>=samplelen[i] && u <= samplelen[i+1]) { u = (u - samplelen[i]) / (samplelen[i+1] - samplelen[i]); bezierPos.LinearlyInterpolate(presample[i],presample[i+1],u); xAxis.LinearlyInterpolate(presampletan[i],presampletan[i+1],u); break; } } } // compute the scaling (straight interpolation!) CVector3 scl1; scl1.LinearlyInterpolate(scl[index1temp], scl[index2temp],vtemp); scl1.PutX(1); // scl1.PutX(modelXF.GetSclX()); // We need to find another way to manage global scaling // compute the rotation! CRotation rot1,rot2; rot1.SetFromXYZAngles(rot[index1temp]); rot2.SetFromXYZAngles(rot[index2temp]); CQuaternion quat; quat.Slerp(rot1.GetQuaternion(),rot2.GetQuaternion(),vtemp); rot1.SetFromQuaternion(quat); yAxis.Set(0,1,0); yAxis.MulByMatrix3InPlace(rot1.GetMatrix()); // use directly or project the roll values! if (mode == 1) { double thisRoll = roll[index1temp] * (1.0 - vtemp) + roll[index2temp] * vtemp; rot1.SetFromAxisAngle(xAxis,DegreesToRadians(thisRoll)); yAxis.MulByMatrix3InPlace(rot1.GetMatrix()); } zAxis.Cross(xAxis,yAxis); yAxis.Cross(zAxis,xAxis); xAxis.NormalizeInPlace(); yAxis.NormalizeInPlace(); zAxis.NormalizeInPlace(); CTransformation result; result.SetScaling(scl1); result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis); result.SetTranslation(bezierPos); KinematicState outKine(ctxt.GetOutputTarget()); outKine.PutTransform(result); return CStatus::OK; }