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