void n_tentacle::computeSlerp(const MMatrix &matrix1, const MMatrix &matrix2, const MFnNurbsCurve &curve, double parameter, double blendRot, double iniLength, double curveLength, double stretch, double globalScale, int tangentAxis, MVector &outPos, MVector &outRot)
{
//curveLength = curve.length()
double lenRatio = iniLength / curveLength;
MQuaternion quat1;
quat1 = matrix1;
MQuaternion quat2;
quat2 = matrix2;
this->bipolarityCheck(quat1, quat2);
//need to adjust the parameter in order to maintain the length between elements, also for the stretch
MVector tangent;
MPoint pointAtParam;
MPoint finaPos;
double p = lenRatio * parameter * globalScale;
double finalParam = p + (parameter - p) * stretch;
if(curveLength * finalParam > curveLength)
{
double lengthDiff = curveLength - (iniLength * parameter);
double param = curve.knot(curve.numKnots() - 1);
tangent = curve.tangent(param, MSpace::kWorld);
tangent.normalize();
curve.getPointAtParam(param, pointAtParam, MSpace::kWorld);
finaPos = pointAtParam;
pointAtParam += (- tangent) * lengthDiff;
//MGlobal::displayInfo("sdf");
}
else
{
double param = curve.findParamFromLength(curveLength * finalParam);
tangent = curve.tangent(param, MSpace::kWorld);
tangent.normalize();
curve.getPointAtParam(param, pointAtParam, MSpace::kWorld);
}
MQuaternion slerpQuat = slerp(quat1, quat2, blendRot);
MMatrix slerpMatrix = slerpQuat.asMatrix();
int axisId = abs(tangentAxis) - 1;
MVector slerpMatrixYAxis = MVector(slerpMatrix(axisId, 0), slerpMatrix(axisId, 1), slerpMatrix(axisId, 2));
slerpMatrixYAxis.normalize();
if(tangentAxis < 0)
slerpMatrixYAxis = - slerpMatrixYAxis;
double angle = tangent.angle(slerpMatrixYAxis);
MVector axis = slerpMatrixYAxis ^ tangent;
axis.normalize();
MQuaternion rotationToSnapOnCurve(angle, axis);
MQuaternion finalQuat = slerpQuat * rotationToSnapOnCurve;
MEulerRotation finalEuler = finalQuat.asEulerRotation();
outRot.x = finalEuler.x;
outRot.y = finalEuler.y;
outRot.z = finalEuler.z;
outPos = pointAtParam;
}
void rigidBodyNode::computeWorldMatrix(const MPlug& plug, MDataBlock& data)
{
if (!m_rigid_body)
return;
// std::cout << "rigidBodyNode::computeWorldMatrix" << std::endl;
MObject thisObject(thisMObject());
MFnDagNode fnDagNode(thisObject);
MObject update;
MPlug(thisObject, ca_rigidBody).getValue(update);
MPlug(thisObject, ca_rigidBodyParam).getValue(update);
vec3f pos;
quatf rot;
MStatus status;
MFnTransform fnParentTransform(fnDagNode.parent(0, &status));
double mscale[3];
fnParentTransform.getScale(mscale);
m_rigid_body->get_transform(pos, rot);
if(dSolverNode::isStartTime)
{ // allow to edit ptranslation and rotation
MVector mtranslation = fnParentTransform.getTranslation(MSpace::kTransform, &status);
MQuaternion mrotation;
fnParentTransform.getRotation(mrotation, MSpace::kTransform);
float deltaPX = (float)mtranslation.x - pos[0];
float deltaPY = (float)mtranslation.y - pos[1];
float deltaPZ = (float)mtranslation.z - pos[2];
float deltaRX = (float)mrotation.x - rot[1];
float deltaRY = (float)mrotation.y - rot[2];
float deltaRZ = (float)mrotation.z - rot[3];
float deltaRW = (float)mrotation.w - rot[0];
float deltaSq = deltaPX * deltaPX + deltaPY * deltaPY + deltaPZ * deltaPZ
+ deltaRX * deltaRX + deltaRY * deltaRY + deltaRZ * deltaRZ + deltaRW * deltaRW;
if(deltaSq > FLT_EPSILON)
{
m_rigid_body->set_transform(vec3f((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->set_interpolation_transform(vec3f((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->update_constraint();
MDataHandle hInitPos = data.outputValue(ia_initialPosition);
float3 &ihpos = hInitPos.asFloat3();
ihpos[0] = (float)mtranslation.x;
ihpos[1] = (float)mtranslation.y;
ihpos[2] = (float)mtranslation.z;
MDataHandle hInitRot = data.outputValue(ia_initialRotation);
float3 &ihrot = hInitRot.asFloat3();
MEulerRotation newrot(mrotation.asEulerRotation());
ihrot[0] = rad2deg((float)newrot.x);
ihrot[1] = rad2deg((float)newrot.y);
ihrot[2] = rad2deg((float)newrot.z);
}
}
else
{ // if not start time, lock position and rotation for active rigid bodies
float mass = 0.f;
MPlug(thisObject, rigidBodyNode::ia_mass).getValue(mass);
if(mass > 0.f)
{
fnParentTransform.setTranslation(MVector(pos[0], pos[1], pos[2]), MSpace::kTransform);
fnParentTransform.setRotation(MQuaternion(rot[1], rot[2], rot[3], rot[0]));
}
}
float mass = 0.f;
MPlug(thisObject, rigidBodyNode::ia_mass).getValue(mass);
float curMass = m_rigid_body->get_mass();
bool changedMassStatus= false;
if ((curMass > 0.f) != (mass > 0.f))
{
changedMassStatus = true;
}
if (changedMassStatus)
solver_t::remove_rigid_body(m_rigid_body);
m_rigid_body->set_mass(mass);
m_rigid_body->set_inertia((float)mass * m_rigid_body->collision_shape()->local_inertia());
if (changedMassStatus)
solver_t::remove_rigid_body(m_rigid_body);
float restitution = 0.f;
MPlug(thisObject, rigidBodyNode::ia_restitution).getValue(restitution);
m_rigid_body->set_restitution(restitution);
float friction = 0.5f;
MPlug(thisObject, rigidBodyNode::ia_friction).getValue(friction);
m_rigid_body->set_friction(friction);
float linDamp = 0.f;
MPlug(thisObject, rigidBodyNode::ia_linearDamping).getValue(linDamp);
m_rigid_body->set_linear_damping(linDamp);
float angDamp = 0.f;
MPlug(thisObject, rigidBodyNode::ia_angularDamping).getValue(angDamp);
m_rigid_body->set_angular_damping(angDamp);
data.setClean(plug);
//set the scale to the collision shape
m_rigid_body->collision_shape()->set_scale(vec3f((float)mscale[0], (float)mscale[1], (float)mscale[2]));
}