//update the passive rigid bodies by interpolation of the previous and current frame
void dSolverNode::updatePassiveRigidBodies(MPlugArray &rbConnections, std::vector<xforms_t> &xforms, float t)
{
size_t pb = 0;
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;
}
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(!active) {
//do linear interpolation for now
rb->set_transform(xforms[pb].m_x0 + t * (xforms[pb].m_x1 - xforms[pb].m_x0),
normalize(xforms[pb].m_q0 + t * (xforms[pb].m_q1 - xforms[pb].m_q0)));
++pb;
}
} 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) {
for(size_t j = 0; j < rbs.size(); ++j) {
rbs[j]->set_transform(xforms[pb].m_x0 + t * (xforms[pb].m_x1 - xforms[pb].m_x0),
normalize(xforms[pb].m_q0 + t * (xforms[pb].m_q1 - xforms[pb].m_q0)));
++pb;
}
}
}
}
}
MStatus boingRbCmd::setBulletVectorAttribute(MObject node, MString attr, MVector vec) {
//MObject node = rbConnections[i].node();
MFnDagNode fnDagNode(node);
if(fnDagNode.typeId() == boingRBNode::typeId) {
boingRBNode *rbNode = static_cast<boingRBNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if (rb)
{
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return MS::kFailure;
}
MPlug plgMass(node, boingRBNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(active) {
if (attr=="velocity") {
vec3f vel;
vel = vec3f((float)vec.x,(float)vec.y,(float)vec.z);
rb->set_linear_velocity(vel);
} else if (attr=="position") {
vec3f pos;
quatf rot;
rb->get_transform(pos, rot);
pos = vec3f((float)vec.x,(float)vec.y,(float)vec.z);
rb->set_transform(pos, rot);
} else if (attr=="angularVelocity") {
vec3f av;
av = vec3f((float)vec.x,(float)vec.y,(float)vec.z);
rb->set_angular_velocity(av);
}
}
}
}
return MS::kSuccess;
}
MVector boingRbCmd::getBulletVectorAttribute(MObject node, MString attr) {
MVector vec;
MFnDagNode fnDagNode(node);
if(fnDagNode.typeId() == boingRBNode::typeId) {
boingRBNode *rbNode = static_cast<boingRBNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if (rb)
{
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return vec;
}
MPlug plgMass(node, boingRBNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(active) {
if (attr=="velocity") {
vec3f vel;
rb->get_linear_velocity(vel);
vec = MVector((double)vel[0], (double)vel[1], (double)vel[2]);
} else if (attr=="position") {
vec3f pos;
quatf rot;
rb->get_transform(pos, rot);
vec = MVector((double)pos[0], (double)pos[1], (double)pos[2]);
} else if (attr=="angularVelocity") {
vec3f av;
rb->get_angular_velocity(av);
vec = MVector((double)av[0], (double)av[1], (double)av[2]);
}
}
}
}
return vec;
}
//apply fields in the scene from the rigid body
void dSolverNode::applyFields(MPlugArray &rbConnections, float dt)
{
MVectorArray position;
MVectorArray velocity;
MDoubleArray mass;
std::vector<rigid_body_t*> rigid_bodies;
//gather active rigid bodies
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();
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(active) {
rigid_bodies.push_back(rb.get());
}
} 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);
if(active) {
for(size_t j = 0; j < rbs.size(); ++j) {
rigid_bodies.push_back(rbs[j].get());
}
}
}
}
//clear forces and get the properties needed for field computation
for(size_t i = 0; i < rigid_bodies.size(); ++i) {
rigid_bodies[i]->clear_forces();
vec3f pos, vel;
quatf rot;
rigid_bodies[i]->get_transform(pos, rot);
rigid_bodies[i]->get_linear_velocity(vel);
position.append(MVector(pos[0], pos[1], pos[2]));
velocity.append(MVector(vel[0], vel[1], vel[2]));
//TODO
mass.append(1.0);
//
}
//apply the fields to the rigid bodies
MVectorArray force;
for(MItDag it(MItDag::kDepthFirst, MFn::kField); !it.isDone(); it.next()) {
MFnField fnField(it.item());
fnField.getForceAtPoint(position, velocity, mass, force, dt);
for(size_t i = 0; i < rigid_bodies.size(); ++i) {
rigid_bodies[i]->apply_central_force(vec3f(force[i].x, force[i].y, force[i].z));
}
}
}
//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);
}
}
