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;
}
