//standard attributes
void sixdofConstraintNode::computeConstraint(const MPlug& plug, MDataBlock& data)
{
// std::cout << "sixdofConstraintNode::computeConstraint" << std::endl;
MObject thisObject(thisMObject());
MPlug plgRigidBodyA(thisObject, ia_rigidBodyA);
MPlug plgRigidBodyB(thisObject, ia_rigidBodyB);
MObject update;
//force evaluation of the rigidBody
plgRigidBodyA.getValue(update);
plgRigidBodyB.getValue(update);
rigid_body_t::pointer rigid_bodyA;
if(plgRigidBodyA.isConnected()) {
MPlugArray connections;
plgRigidBodyA.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeA(connections[0].node());
if(fnNodeA.typeId() == rigidBodyNode::typeId) {
rigidBodyNode *pRigidBodyNodeA = static_cast<rigidBodyNode*>(fnNodeA.userNode());
rigid_bodyA = pRigidBodyNodeA->rigid_body();
} else {
std::cout << "sixdofConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
rigid_body_t::pointer rigid_bodyB;
if(plgRigidBodyB.isConnected()) {
MPlugArray connections;
plgRigidBodyB.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeB(connections[0].node());
if(fnNodeB.typeId() == rigidBodyNode::typeId) {
rigidBodyNode *pRigidBodyNodeB = static_cast<rigidBodyNode*>(fnNodeB.userNode());
rigid_bodyB = pRigidBodyNodeB->rigid_body();
} else {
std::cout << "sixdofConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
if(rigid_bodyA && rigid_bodyB) {
//not connected to a rigid body, put a default one
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::remove_constraint(constraint);
m_constraint = solver_t::create_sixdof_constraint(rigid_bodyA, vec3f(), rigid_bodyB, vec3f());
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint);
}
data.outputValue(ca_constraint).set(true);
data.setClean(plug);
}
void sixdofConstraintNode::computeWorldMatrix(const MPlug& plug, MDataBlock& data)
{
MObject thisObject(thisMObject());
MFnDagNode fnDagNode(thisObject);
MObject update;
MPlug(thisObject, ca_constraint).getValue(update);
MPlug(thisObject, ca_constraintParam).getValue(update);
MStatus status;
MFnTransform fnParentTransform(fnDagNode.parent(0, &status));
double fixScale[3] = { 1., 1., 1. }; // lock scale
fnParentTransform.setScale(fixScale);
MVector mtranslation = fnParentTransform.getTranslation(MSpace::kTransform, &status);
if(bSolverNode::isStartTime)
{ // allow to edit pivots
MPlug plgRigidBodyA(thisObject, ia_rigidBodyA);
MPlug plgRigidBodyB(thisObject, ia_rigidBodyB);
MObject update;
//force evaluation of the rigidBody
plgRigidBodyA.getValue(update);
if(plgRigidBodyA.isConnected())
{
MPlugArray connections;
plgRigidBodyA.connectedTo(connections, true, true);
if(connections.length() != 0)
{
MFnDependencyNode fnNode(connections[0].node());
if(fnNode.typeId() == boingRBNode::typeId)
{
MObject rbAObj = fnNode.object();
boingRBNode *pRigidBodyNodeA = static_cast<boingRBNode*>(fnNode.userNode());
MPlug(rbAObj, pRigidBodyNodeA->worldMatrix).elementByLogicalIndex(0).getValue(update);
}
}
}
plgRigidBodyB.getValue(update);
if(plgRigidBodyB.isConnected())
{
MPlugArray connections;
plgRigidBodyB.connectedTo(connections, true, true);
if(connections.length() != 0)
{
MFnDependencyNode fnNode(connections[0].node());
if(fnNode.typeId() == boingRBNode::typeId)
{
MObject rbBObj = fnNode.object();
boingRBNode *pRigidBodyNodeB = static_cast<boingRBNode*>(fnNode.userNode());
MPlug(rbBObj, pRigidBodyNodeB->worldMatrix).elementByLogicalIndex(0).getValue(update);
}
}
}
if(m_constraint)
{
MQuaternion mrotation;
fnParentTransform.getRotation(mrotation, MSpace::kTransform);
bool doUpdatePivot = m_constraint->getPivotChanged();
if(!doUpdatePivot)
{
vec3f worldP;
quatf worldR;
m_constraint->get_world(worldP, worldR);
float deltaPX = worldP[0] - float(mtranslation.x);
float deltaPY = worldP[1] - float(mtranslation.y);
float deltaPZ = worldP[2] - float(mtranslation.z);
float deltaRX = (float)mrotation.x - worldR[1];
float deltaRY = (float)mrotation.y - worldR[2];
float deltaRZ = (float)mrotation.z - worldR[3];
float deltaRW = (float)mrotation.w - worldR[0];
float deltaSq = deltaPX * deltaPX + deltaPY * deltaPY + deltaPZ * deltaPZ
+ deltaRX * deltaRX + deltaRY * deltaRY + deltaRZ * deltaRZ + deltaRW * deltaRW;
doUpdatePivot = (deltaSq > FLT_EPSILON);
}
if(doUpdatePivot)
{
m_constraint->set_world(vec3f((float) mtranslation[0], (float) mtranslation[1], (float) mtranslation[2]),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
vec3f pivInA, pivInB;
quatf rotInA, rotInB;
m_constraint->get_frameA(pivInA, rotInA);
m_constraint->get_frameB(pivInB, rotInB);
MDataHandle hPivInA = data.outputValue(ia_pivotInA);
float3 &ihPivInA = hPivInA.asFloat3();
MDataHandle hPivInB = data.outputValue(ia_pivotInB);
float3 &ihPivInB = hPivInB.asFloat3();
for(int i = 0; i < 3; i++)
{
ihPivInA[i] = pivInA[i];
ihPivInB[i] = pivInB[i];
}
MDataHandle hRotInA = data.outputValue(ia_rotationInA);
float3 &hrotInA = hRotInA.asFloat3();
MQuaternion mrotA(rotInA[1], rotInA[2], rotInA[3], rotInA[0]);
MEulerRotation newrotA(mrotA.asEulerRotation());
hrotInA[0] = rad2deg((float)newrotA.x);
hrotInA[1] = rad2deg((float)newrotA.y);
hrotInA[2] = rad2deg((float)newrotA.z);
MDataHandle hRotInB = data.outputValue(ia_rotationInB);
float3 &hrotInB = hRotInB.asFloat3();
MQuaternion mrotB(rotInB[1], rotInB[2], rotInB[3], rotInB[0]);
MEulerRotation newrotB(mrotB.asEulerRotation());
hrotInB[0] = rad2deg((float)newrotB.x);
hrotInB[1] = rad2deg((float)newrotB.y);
hrotInB[2] = rad2deg((float)newrotB.z);
m_constraint->setPivotChanged(false);
m_constraint->get_local_frameA(m_PivInA, m_RotInA);
m_constraint->get_local_frameB(m_PivInB, m_RotInB);
}
}
}
else
{ // if not start time, lock position and rotation
if(m_constraint)
{
vec3f worldP;
quatf worldR;
m_constraint->get_world(worldP, worldR);
fnParentTransform.setTranslation(MVector(worldP[0], worldP[1], worldP[2]), MSpace::kTransform);
fnParentTransform.setRotation(MQuaternion(worldR[1], worldR[2], worldR[3], worldR[0]));
}
}
m_initialized = true;
data.setClean(plug);
}
//standard attributes
void sixdofConstraintNode::computeConstraint(const MPlug& plug, MDataBlock& data)
{
// std::cout << "sixdofConstraintNode::computeConstraint" << std::endl;
MObject thisObject(thisMObject());
MPlug plgRigidBodyA(thisObject, ia_rigidBodyA);
MPlug plgRigidBodyB(thisObject, ia_rigidBodyB);
MObject update;
//force evaluation of the rigidBody
plgRigidBodyA.getValue(update);
plgRigidBodyB.getValue(update);
rigid_body_t::pointer rigid_bodyA;
if(plgRigidBodyA.isConnected()) {
MPlugArray connections;
plgRigidBodyA.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeA(connections[0].node());
if(fnNodeA.typeId() == boingRBNode::typeId) {
boingRBNode *pRigidBodyNodeA = static_cast<boingRBNode*>(fnNodeA.userNode());
rigid_bodyA = pRigidBodyNodeA->rigid_body();
} else {
std::cout << "sixdofConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
rigid_body_t::pointer rigid_bodyB;
if(plgRigidBodyB.isConnected()) {
MPlugArray connections;
plgRigidBodyB.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeB(connections[0].node());
if(fnNodeB.typeId() == boingRBNode::typeId) {
boingRBNode *pRigidBodyNodeB = static_cast<boingRBNode*>(fnNodeB.userNode());
rigid_bodyB = pRigidBodyNodeB->rigid_body();
} else {
std::cout << "sixdofConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
vec3f pivInA, pivInB;
if((rigid_bodyA != NULL) && (rigid_bodyB != NULL))
{
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::remove_constraint(constraint);
float3& mPivInA = data.inputValue(ia_pivotInA).asFloat3();
float3& mPivInB = data.inputValue(ia_pivotInB).asFloat3();
for(int i = 0; i < 3; i++)
{
pivInA[i] = (float)mPivInA[i];
pivInB[i] = (float)mPivInB[i];
}
float3& mRotInA = data.inputValue(ia_rotationInA).asFloat3();
MEulerRotation meulerA(deg2rad(mRotInA[0]), deg2rad(mRotInA[1]), deg2rad(mRotInA[2]));
MQuaternion mquatA = meulerA.asQuaternion();
quatf rotA((float)mquatA.w, (float)mquatA.x, (float)mquatA.y, (float)mquatA.z);
float3& mRotInB = data.inputValue(ia_rotationInB).asFloat3();
MEulerRotation meulerB(deg2rad(mRotInB[0]), deg2rad(mRotInB[1]), deg2rad(mRotInB[2]));
MQuaternion mquatB = meulerB.asQuaternion();
quatf rotB((float)mquatB.w, (float)mquatB.x, (float)mquatB.y, (float)mquatB.z);
m_constraint = solver_t::create_sixdof_constraint(rigid_bodyA, pivInA, rotA, rigid_bodyB, pivInB, rotB);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, data.inputValue(ia_disableCollide).asBool());
}
else if(rigid_bodyA != NULL)
{
//not connected to a rigid body, put a default one
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::remove_constraint(constraint);
float3& mPivInA = data.inputValue(ia_pivotInA).asFloat3();
for(int i = 0; i < 3; i++)
{
pivInA[i] = (float)mPivInA[i];
}
float3& mRotInA = data.inputValue(ia_rotationInA).asFloat3();
MEulerRotation meuler(deg2rad(mRotInA[0]), deg2rad(mRotInA[1]), deg2rad(mRotInA[2]));
MQuaternion mquat = meuler.asQuaternion();
quatf rotA((float)mquat.w, (float)mquat.x, (float)mquat.y, (float)mquat.z);
m_constraint = solver_t::create_sixdof_constraint(rigid_bodyA, pivInA, rotA);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, data.inputValue(ia_disableCollide).asBool());
}
if (m_constraint)
{
m_constraint->get_local_frameA(m_PivInA, m_RotInA);
m_constraint->get_local_frameB(m_PivInB, m_RotInB);
}
data.outputValue(ca_constraint).set(true);
data.setClean(plug);
}
//standard attributes
void sixdofConstraintNode::reComputeConstraint(const MPlug& plug, MDataBlock& data1)
{
if (!m_initialized)
return;
// std::cout << "sixdofConstraintNode::computeConstraint" << std::endl;
m_disableCollision = data1.inputValue(ia_disableCollide).asBool();
MObject thisObject(thisMObject());
MPlug plgRigidBodyA(thisObject, ia_rigidBodyA);
MPlug plgRigidBodyB(thisObject, ia_rigidBodyB);
MObject update;
//force evaluation of the rigidBody
plgRigidBodyA.getValue(update);
plgRigidBodyB.getValue(update);
rigid_body_t::pointer rigid_bodyA;
if(plgRigidBodyA.isConnected()) {
MPlugArray connections;
plgRigidBodyA.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeA(connections[0].node());
if(fnNodeA.typeId() == boingRBNode::typeId) {
boingRBNode *pRigidBodyNodeA = static_cast<boingRBNode*>(fnNodeA.userNode());
rigid_bodyA = pRigidBodyNodeA->rigid_body();
} else {
std::cout << "sixdofConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
rigid_body_t::pointer rigid_bodyB;
if(plgRigidBodyB.isConnected()) {
MPlugArray connections;
plgRigidBodyB.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeB(connections[0].node());
if(fnNodeB.typeId() == boingRBNode::typeId) {
boingRBNode *pRigidBodyNodeB = static_cast<boingRBNode*>(fnNodeB.userNode());
rigid_bodyB = pRigidBodyNodeB->rigid_body();
} else {
std::cout << "sixdofConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
if((rigid_bodyA != NULL) && (rigid_bodyB != NULL))
{
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
bt_sixdof_constraint_t* sixdof_impl = dynamic_cast<bt_sixdof_constraint_t*>(constraint->pubImpl());
rigid_bodyA->remove_constraint(sixdof_impl);
rigid_bodyB->remove_constraint(sixdof_impl);
solver_t::remove_constraint(constraint);
m_constraint = solver_t::create_sixdof_constraint(rigid_bodyA, m_PivInA, m_RotInA, rigid_bodyB, m_PivInB, m_RotInB);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, m_disableCollision);
}
else if(rigid_bodyA != NULL)
{
//not connected to a rigid body, put a default one
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
bt_sixdof_constraint_t* sixdof_impl = dynamic_cast<bt_sixdof_constraint_t*>(constraint->pubImpl());
rigid_bodyA->remove_constraint(sixdof_impl);
solver_t::remove_constraint(constraint);
m_constraint = solver_t::create_sixdof_constraint(rigid_bodyA, m_PivInB, m_RotInB);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, m_disableCollision);
}else if(rigid_bodyB != NULL)
{
//not connected to a rigid body, put a default one
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
bt_sixdof_constraint_t* sixdof_impl = dynamic_cast<bt_sixdof_constraint_t*>(constraint->pubImpl());
rigid_bodyB->remove_constraint(sixdof_impl);
solver_t::remove_constraint(constraint);
m_constraint = solver_t::create_sixdof_constraint(rigid_bodyB, m_PivInA, m_RotInA);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, m_disableCollision);
}
float val = 0.f;
MPlug(thisObject, sixdofConstraintNode::ia_damping).getValue(val);
m_constraint->set_damping(val);
MPlug(thisObject, sixdofConstraintNode::ia_breakThreshold).getValue(val);
m_constraint->set_breakThreshold(val);
vec3f lowLin, uppLin, lowAng, uppAng;
for (int i=0;i<3;i++)
{
lowLin[i] = 0.f;
uppLin[i] = 0.f;
lowAng[i] = 0.f;
uppAng[i] = 0.f;
}
{
MPlug ll(thisObject, sixdofConstraintNode::ia_lowerLinLimit);
if (ll.isCompound())
{
for (int i=0;i<3;i++)
{
ll.child(i).getValue(lowLin[i]);
}
}
}
{
MPlug ul(thisObject, sixdofConstraintNode::ia_upperLinLimit);
if (ul.isCompound())
{
for (int i=0;i<3;i++)
{
ul.child(i).getValue(uppLin[i]);
}
}
}
{
MPlug all(thisObject, sixdofConstraintNode::ia_lowerAngLimit);
if (all.isCompound())
{
for (int i=0;i<3;i++)
{
float f;
all.child(i).getValue(f);
lowAng[i] = deg2rad(f);
}
}
}
{
MPlug aul(thisObject, sixdofConstraintNode::ia_upperAngLimit);
if (aul.isCompound())
{
for (int i=0;i<3;i++)
{
float f;
aul.child(i).getValue(f);
uppAng[i] = deg2rad(f);
}
}
}
m_constraint->set_LinLimit(lowLin, uppLin);
m_constraint->set_AngLimit(lowAng, uppAng);
data1.outputValue(ca_constraint).set(true);
data1.setClean(plug);
}
//standard attributes
void sliderConstraintNode::reComputeConstraint()
{
if (!m_initialized)
return;
// std::cout << "sliderConstraintNode::computeConstraint" << std::endl;
MObject thisObject(thisMObject());
MPlug plgRigidBodyA(thisObject, ia_rigidBodyA);
MPlug plgRigidBodyB(thisObject, ia_rigidBodyB);
MObject update;
//force evaluation of the rigidBody
plgRigidBodyA.getValue(update);
plgRigidBodyB.getValue(update);
rigid_body_t::pointer rigid_bodyA;
if(plgRigidBodyA.isConnected()) {
MPlugArray connections;
plgRigidBodyA.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeA(connections[0].node());
if(fnNodeA.typeId() == boingRBNode::typeId) {
boingRBNode *pRigidBodyNodeA = static_cast<boingRBNode*>(fnNodeA.userNode());
rigid_bodyA = pRigidBodyNodeA->rigid_body();
} else {
std::cout << "sliderConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
rigid_body_t::pointer rigid_bodyB;
if(plgRigidBodyB.isConnected()) {
MPlugArray connections;
plgRigidBodyB.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNodeB(connections[0].node());
if(fnNodeB.typeId() == boingRBNode::typeId) {
boingRBNode *pRigidBodyNodeB = static_cast<boingRBNode*>(fnNodeB.userNode());
rigid_bodyB = pRigidBodyNodeB->rigid_body();
} else {
std::cout << "sliderConstraintNode connected to a non-rigidbody node!" << std::endl;
}
}
}
if((rigid_bodyA != NULL) && (rigid_bodyB != NULL))
{
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
if (constraint)
{
bt_slider_constraint_t* hinge_impl = dynamic_cast<bt_slider_constraint_t*>(constraint->pubImpl());
rigid_bodyA->remove_constraint(hinge_impl);
rigid_bodyB->remove_constraint(hinge_impl);
solver_t::remove_constraint(constraint);
}
m_constraint = solver_t::create_slider_constraint(rigid_bodyA, m_PivInA, m_RotInA, rigid_bodyB, m_PivInB, m_RotInB);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, m_disableCollision);
}
else if(rigid_bodyA != NULL)
{
//not connected to a rigid body, put a default one
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
if (constraint)
{
bt_slider_constraint_t* hinge_impl = dynamic_cast<bt_slider_constraint_t*>(constraint->pubImpl());
rigid_bodyA->remove_constraint(hinge_impl);
solver_t::remove_constraint(constraint);
}
m_constraint = solver_t::create_slider_constraint(rigid_bodyA, m_PivInB, m_RotInB);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, m_disableCollision);
} else if (rigid_bodyB != NULL)
{
//not connected to a rigid body, put a default one
constraint_t::pointer constraint = static_cast<constraint_t::pointer>(m_constraint);
if (constraint)
{
bt_slider_constraint_t* hinge_impl = dynamic_cast<bt_slider_constraint_t*>(constraint->pubImpl());
rigid_bodyB->remove_constraint(hinge_impl);
solver_t::remove_constraint(constraint);
}
m_constraint = solver_t::create_slider_constraint(rigid_bodyB, m_PivInA, m_RotInA);
constraint = static_cast<constraint_t::pointer>(m_constraint);
solver_t::add_constraint(constraint, m_disableCollision);
}
if (m_constraint)
{
float val = 0.f;
MPlug(thisObject, sliderConstraintNode::ia_damping).getValue(val);
m_constraint->set_damping(val);
MPlug(thisObject, sliderConstraintNode::ia_breakThreshold).getValue(val);
m_constraint->set_breakThreshold(val);
float lin_lower=0.f;
float lin_upper=0.f;
MPlug(thisObject, sliderConstraintNode::ia_lowerLinLimit).getValue(lin_lower);
MPlug(thisObject, sliderConstraintNode::ia_upperLinLimit).getValue(lin_upper);
m_constraint->set_LinLimit(lin_lower, lin_upper);
float ang_lower=0.f;
float ang_upper=0.f;
MPlug(thisObject, sliderConstraintNode::ia_lowerAngLimit).getValue(ang_lower);
MPlug(thisObject, sliderConstraintNode::ia_upperAngLimit).getValue(ang_upper);
m_constraint->set_AngLimit(deg2rad(ang_lower), deg2rad(ang_upper));
m_constraint->setPivotChanged(true);
}
}
