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