void nailConstraintNode::computeWorldMatrix(const MPlug& plug, MDataBlock& data)
{
// std::cout << "nailConstraintNode::computeWorldMatrix" << std::endl;
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));
MVector mtranslation = fnParentTransform.getTranslation(MSpace::kTransform, &status);
// MQuaternion mrotation;
// fnParentTransform.getRotation(mrotation, MSpace::kTransform);
if(m_constraint) {
vec3f world;
m_constraint->get_world(world);
if(world[0] != float(mtranslation.x) ||
world[1] != float(mtranslation.y) ||
world[2] != float(mtranslation.z)) {
/* mat4x4f xform;
m_constraint->rigid_body()->get_transform(xform);
vec4f pivot = prod(trans(xform), vec4f(mtranslation.x, mtranslation.y, mtranslation.z, 1.0));
std::cout << "pivot (" << pivot[0] << "," << pivot[0] << "," << pivot[0] << ")" << std::endl;
*/
// std::cout << "mtranslation (" << mtranslation[0] << "," << mtranslation[0] << "," << mtranslation[0] << ")" << std::endl;
m_constraint->set_world(vec3f((float) mtranslation[0], (float) mtranslation[1], (float) mtranslation[2]));
}
}
data.setClean(plug);
}
collision_shape_t::pointer collisionShapeNode::createCompositeShape(const MPlug& plgInShape)
{
std::vector<collision_shape_t::pointer> childCollisionShapes;
std::vector< vec3f> childPositions;
std::vector< quatf> childOrientations;
MPlugArray plgaConnectedTo;
plgInShape.connectedTo(plgaConnectedTo, true, true);
int numSelectedShapes = plgaConnectedTo.length();
if(numSelectedShapes > 0) {
MObject node = plgaConnectedTo[0].node();
MDagPath dagPath;
MDagPath::getAPathTo(node, dagPath);
int numChildren = dagPath.childCount();
if (node.hasFn(MFn::kTransform))
{
MFnTransform trNode(dagPath);
MVector mtranslation = trNode.getTranslation(MSpace::kTransform);
MObject thisObject(thisMObject());
MFnDagNode fnDagNode(thisObject);
MStatus status;
MFnTransform fnParentTransform(fnDagNode.parent(0, &status));
mtranslation = trNode.getTranslation(MSpace::kPostTransform);
mtranslation = fnParentTransform.getTranslation(MSpace::kTransform);
const char* name = trNode.name().asChar();
printf("name = %s\n", name);
for (int i=0;i<numChildren;i++)
{
MObject child = dagPath.child(i);
if(child.hasFn(MFn::kMesh))
{
return false;
}
if(child.hasFn(MFn::kTransform))
{
MDagPath dagPath;
MDagPath::getAPathTo(child, dagPath);
MFnTransform trNode(dagPath);
MVector mtranslation = trNode.getTranslation(MSpace::kTransform);
mtranslation = trNode.getTranslation(MSpace::kPostTransform);
MQuaternion mrotation;
trNode.getRotation(mrotation, MSpace::kTransform);
double mscale[3];
trNode.getScale(mscale);
vec3f childPos((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z);
quatf childOrn((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z);
const char* name = trNode.name().asChar();
printf("name = %s\n", name);
int numGrandChildren = dagPath.childCount();
{
for (int i=0;i<numGrandChildren;i++)
{
MObject grandchild = dagPath.child(i);
if(grandchild.hasFn(MFn::kMesh))
{
collision_shape_t::pointer childShape = createCollisionShape(grandchild);
if (childShape)
{
childCollisionShapes.push_back(childShape);
childPositions.push_back(childPos);
childOrientations.push_back(childOrn);
}
}
}
}
}
}
}
}
if (childCollisionShapes.size()>0)
{
composite_shape_t::pointer composite = solver_t::create_composite_shape(
&childCollisionShapes[0],
&childPositions[0],
&childOrientations[0],
childCollisionShapes.size()
);
return composite;
}
return 0;
}
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);
}
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]));
}
