static void state_inverse(double *I, const state *s)
{
double T[16], A[16], R[16];
mtranslate(T, s->p);
meuler (A, s->e);
mtranspose(R, A);
mmultiply (I, T, R);
}
static void state_matrix(double *M, const state *s)
{
double T[16], R[16], p[3];
vneg(p, s->p);
mtranslate(T, p);
meuler (R, s->e);
mmultiply (M, R, T);
}
//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);
}
//update the scene after a simulation step
void dSolverNode::updateActiveRigidBodies(MPlugArray &rbConnections)
{
//update the active rigid bodies to the new configuration
for(size_t i = 0; i < rbConnections.length(); ++i) {
MObject node = rbConnections[i].node();
MFnDagNode fnDagNode(node);
if(fnDagNode.typeId() == rigidBodyNode::typeId) {
rigidBodyNode *rbNode = static_cast<rigidBodyNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
continue;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(active) {
quatf rot;
vec3f pos;
rb->get_transform(pos, rot);
fnTransform.setRotation(MQuaternion(rot[1], rot[2], rot[3], rot[0]));
fnTransform.setTranslation(MVector(pos[0], pos[1], pos[2]), MSpace::kTransform);
}
} else if(fnDagNode.typeId() == rigidBodyArrayNode::typeId) {
rigidBodyArrayNode *rbNode = static_cast<rigidBodyArrayNode*>(fnDagNode.userNode());
std::vector<rigid_body_t::pointer>& rbs = rbNode->rigid_bodies();
MPlug plgMass(node, rigidBodyArrayNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
//write the position and rotations
if(active) {
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
vec3f pos;
quatf rot;
rbs[j]->get_transform(pos, rot);
MEulerRotation meuler(MQuaternion(rot[1], rot[2], rot[3], rot[0]).asEulerRotation());
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).setValue(pos[0]);
plgElement.child(1).setValue(pos[1]);
plgElement.child(2).setValue(pos[2]);
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).setValue(rad2deg(meuler.x));
plgElement.child(1).setValue(rad2deg(meuler.y));
plgElement.child(2).setValue(rad2deg(meuler.z));
}
}
//check if we have to output the rigid bodies to a file
MPlug plgFileIO(node, rigidBodyArrayNode::ia_fileIO);
bool doIO;
plgFileIO.getValue(doIO);
if(doIO) {
dumpRigidBodyArray(node);
}
}
}
}
//gather previous and current frame transformations for substep interpolation
void dSolverNode::gatherPassiveTransforms(MPlugArray &rbConnections, std::vector<xforms_t> &xforms)
{
xforms.resize(0);
xforms_t xform;
for(size_t i = 0; i < rbConnections.length(); ++i) {
MObject node = rbConnections[i].node();
MFnDagNode fnDagNode(node);
if(fnDagNode.typeId() == rigidBodyNode::typeId) {
rigidBodyNode *rbNode = static_cast<rigidBodyNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
continue;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(!active) {
MQuaternion mquat;
fnTransform.getRotation(mquat);
MVector mpos(fnTransform.getTranslation(MSpace::kTransform));
rb->get_transform(xform.m_x0, xform.m_q0);
xform.m_x1 = vec3f(mpos.x, mpos.y, mpos.z);
xform.m_q1 = quatf(mquat.w, mquat.x, mquat.y, mquat.z);
xforms.push_back(xform);
}
} else if(fnDagNode.typeId() == rigidBodyArrayNode::typeId) {
rigidBodyArrayNode *rbNode = static_cast<rigidBodyArrayNode*>(fnDagNode.userNode());
std::vector<rigid_body_t::pointer>& rbs = rbNode->rigid_bodies();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return;
}
MPlug plgMass(node, rigidBodyArrayNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(!active) {
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
rbs[j]->get_transform(xform.m_x0, xform.m_q0);
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).getValue(xform.m_x1[0]);
plgElement.child(1).getValue(xform.m_x1[1]);
plgElement.child(2).getValue(xform.m_x1[2]);
vec3f rot;
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).getValue(rot[0]);
plgElement.child(1).getValue(rot[1]);
plgElement.child(2).getValue(rot[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
xform.m_q1 = quatf(mquat.w, mquat.x, mquat.y, mquat.z);
xforms.push_back(xform);
}
}
}
}
}
void initRigidBodyArray(MObject &node)
{
MFnDagNode fnDagNode(node);
rigidBodyArrayNode *rbNode = static_cast<rigidBodyArrayNode*>(fnDagNode.userNode());
std::vector<rigid_body_t::pointer>& rbs = rbNode->rigid_bodies();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyArrayNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(active) {
//active rigid body, set the world transform from the initial* attributes
MObject obj;
MPlug plgInitialPosition(node, rigidBodyArrayNode::ia_initialPosition);
MPlug plgInitialRotation(node, rigidBodyArrayNode::ia_initialRotation);
MPlug plgInitialVelocity(node, rigidBodyArrayNode::ia_initialVelocity);
MPlug plgInitialSpin(node, rigidBodyArrayNode::ia_initialSpin);
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
vec3f pos;
plgElement = plgInitialPosition.elementByLogicalIndex(j);
plgElement.child(0).getValue(pos[0]);
plgElement.child(1).getValue(pos[1]);
plgElement.child(2).getValue(pos[2]);
vec3f rot;
plgElement = plgInitialRotation.elementByLogicalIndex(j);
plgElement.child(0).getValue(rot[0]);
plgElement.child(1).getValue(rot[1]);
plgElement.child(2).getValue(rot[2]);
vec3f vel;
plgElement = plgInitialVelocity.elementByLogicalIndex(j);
plgElement.child(0).getValue(vel[0]);
plgElement.child(1).getValue(vel[1]);
plgElement.child(2).getValue(vel[2]);
vec3f spin;
plgElement = plgInitialSpin.elementByLogicalIndex(j);
plgElement.child(0).getValue(spin[0]);
plgElement.child(1).getValue(spin[1]);
plgElement.child(2).getValue(spin[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
rbs[j]->set_transform(pos, quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rbs[j]->set_linear_velocity(vel);
rbs[j]->set_angular_velocity(spin);
rbs[j]->set_kinematic(false);
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).setValue(pos[0]);
plgElement.child(1).setValue(pos[1]);
plgElement.child(2).setValue(pos[2]);
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).setValue(rot[0]);
plgElement.child(1).setValue(rot[1]);
plgElement.child(2).setValue(rot[2]);
}
} else {
//passive rigid body, get the world trasform from from the position/rotation attributes
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
vec3f pos;
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).getValue(pos[0]);
plgElement.child(1).getValue(pos[1]);
plgElement.child(2).getValue(pos[2]);
vec3f rot;
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).getValue(rot[0]);
plgElement.child(1).getValue(rot[1]);
plgElement.child(2).getValue(rot[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
rbs[j]->set_transform(pos, quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rbs[j]->set_kinematic(false);
}
}
}
void initRigidBody(MObject &node)
{
MFnDagNode fnDagNode(node);
rigidBodyNode *rbNode = static_cast<rigidBodyNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
if(mass>0.f) {
//active rigid body, set the world transform from the initial* attributes
MObject obj;
vec3f pos;
MPlug plgInitialValue(node, rigidBodyNode::ia_initialPosition);
plgInitialValue.child(0).getValue(pos[0]);
plgInitialValue.child(1).getValue(pos[1]);
plgInitialValue.child(2).getValue(pos[2]);
vec3f rot;
plgInitialValue.setAttribute(rigidBodyNode::ia_initialRotation);
plgInitialValue.child(0).getValue(rot[0]);
plgInitialValue.child(1).getValue(rot[1]);
plgInitialValue.child(2).getValue(rot[2]);
vec3f vel;
plgInitialValue.setAttribute(rigidBodyNode::ia_initialVelocity);
plgInitialValue.child(0).getValue(vel[0]);
plgInitialValue.child(1).getValue(vel[1]);
plgInitialValue.child(2).getValue(vel[2]);
vec3f spin;
plgInitialValue.setAttribute(rigidBodyNode::ia_initialSpin);
plgInitialValue.child(0).getValue(spin[0]);
plgInitialValue.child(1).getValue(spin[1]);
plgInitialValue.child(2).getValue(spin[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
rb->set_transform(pos, quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rb->set_linear_velocity(vel);
rb->set_angular_velocity(spin);
rb->set_kinematic(false);
fnTransform.setRotation(meuler);
fnTransform.setTranslation(MVector(pos[0], pos[1], pos[2]), MSpace::kTransform);
} else {
//passive rigid body, get the world trasform from from the shape node
MQuaternion mquat;
fnTransform.getRotation(mquat);
MVector mpos(fnTransform.getTranslation(MSpace::kTransform));
rb->set_transform(vec3f(mpos.x, mpos.y, mpos.z), quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rb->set_kinematic(true);
}
}
