void MObject3d::setEulerRotation(const MVector3 & euler)
{
MQuaternion rotation = MQuaternion(euler.x, euler.y, euler.z);
if(rotation != m_rotation)
{
m_rotation = rotation;
m_needToUpdate = true;
}
}
MQuaternion MQuaternion::operator * (const MQuaternion & quat) const
{
return MQuaternion(
(values[3] * quat.values[0]) + (values[0] * quat.values[3]) + (values[1] * quat.values[2]) - (values[2] * quat.values[1]),
(values[3] * quat.values[1]) + (values[1] * quat.values[3]) + (values[2] * quat.values[0]) - (values[0] * quat.values[2]),
(values[3] * quat.values[2]) + (values[2] * quat.values[3]) + (values[0] * quat.values[1]) - (values[1] * quat.values[0]),
(values[3] * quat.values[3]) - (values[0] * quat.values[0]) - (values[1] * quat.values[1]) - (values[2] * quat.values[2])
);
}
void MObject3d::setAxisAngleRotation(const MVector3 & axis, const float angle)
{
MQuaternion rotation = MQuaternion(angle, axis);
if(rotation != m_rotation)
{
m_rotation = rotation;
m_needToUpdate = true;
}
}
void MScene::prepareCollisionShape(MOEntity * entity)
{
MPhysicsContext * physics = MEngine::getInstance()->getPhysicsContext();
MPhysicsProperties * phyProps = entity->getPhysicsProperties();
if(! phyProps)
return;
unsigned int shapeId = 0;
if(createShape(entity, phyProps, &shapeId))
{
// has physics child
bool hasPhysicsChild = false;
unsigned int o;
unsigned int oSize = entity->getChildsNumber();
for(o=0; o<oSize; o++)
{
MObject3d * childObject = entity->getChild(o);
if(childObject->getType() == M_OBJECT3D_ENTITY)
{
MOEntity * childEntity = (MOEntity*)childObject;
MPhysicsProperties * childPhyProps = childEntity->getPhysicsProperties();
if(childPhyProps)
{
if(! childPhyProps->isGhost())
{
hasPhysicsChild = true;
break;
}
}
}
}
// create multi shape
if(hasPhysicsChild)
{
unsigned int subShapeId = shapeId;
physics->createMultiShape(&shapeId);
physics->addChildShape(shapeId, subShapeId, MVector3(), MQuaternion());
}
phyProps->setShapeId(shapeId);
}
}
MQuaternion convert( const Imath::Quatd &from )
{
return MQuaternion( from[1], from[2], from[3], from[0] );
}
MQuaternion convert( const Imath::Quatf &from )
{
return MQuaternion( static_cast<double>(from[1]), static_cast<double>(from[2]), static_cast<double>(from[3]), static_cast<double>(from[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);
}
bool animateQuaternion(MKey * keys, unsigned int keysNumber, float t, MQuaternion * quaternion)
{
// no keys
if (keysNumber == 0)
return false;
// one key
if (keysNumber == 1)
{
(*quaternion) = *((MQuaternion *)keys->getData());
return true;
}
// out of range
MKey * keyMin = keys;
MKey * keyMax = keys + (keysNumber - 1);
int tMin = keyMin->getT();
int tMax = keyMax->getT();
if (t <= tMin)
{
(*quaternion) = *((MQuaternion *)keyMin->getData());
return true;
}
if (t >= tMax)
{
(*quaternion) = *((MQuaternion *)keyMax->getData());
return true;
}
// interpolation
for (unsigned int i = 1; i < keysNumber; i++)
{
MKey * key0 = keys;
MKey * key1 = keys+1;
int t0 = key0->getT();
int t1 = key1->getT();
if (t == t0)
{
(*quaternion) = *(MQuaternion *)key0->getData();
return true;
}
if (t == t1)
{
(*quaternion) = *(MQuaternion *)key1->getData();
return true;
}
if ((t > t0) && (t < t1))
{
float factor = (t - t0) / (float)(t1 - t0);
MQuaternion * data0 = (MQuaternion *)key0->getData();
MQuaternion * data1 = (MQuaternion *)key1->getData();
(*quaternion) = MQuaternion(*data0, *data1, factor);
return true;
}
keys++;
}
return false;
}
void MObject3d::addAxisAngleRotation(const MVector3 & axis, const float angle)
{
M_PROFILE_SCOPE(MObject3d::addAxisAngleRotation);
m_rotation *= MQuaternion(angle, axis);
m_needToUpdate = true;
}
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]));
}
MStatus
HRBFSkinCluster::skinDQ(MMatrixArray& transforms,
int numTransforms,
MArrayDataHandle& weightListHandle,
MItGeometry& iter) {
MStatus returnStatus;
// compute dual quaternions. we're storing them as a parallel array.
std::vector<MQuaternion> tQuaternions(numTransforms); // translation quaterions
std::vector<MQuaternion> rQuaternions(numTransforms); // rotation quaternions
for (int i = 0; i < numTransforms; i++) {
rQuaternions.at(i) = getRotationQuaternion(transforms[i]);
rQuaternions.at(i).normalizeIt();
tQuaternions.at(i) = getTranslationQuaternion(transforms[i], rQuaternions.at(i));
#if DEBUG_PRINTS
std::cout << "rota quaternion " << i << " is: " << rQuaternions.at(i) << std::endl;
std::cout << "tran quaternion " << i << " is: " << tQuaternions.at(i) << std::endl;
#endif
}
MQuaternion rBlend; // blended rotation quaternions
MQuaternion tBlend; // blended translation quaternions
MQuaternion scaleMe; // Maya's quaternion scaling is in-place
double weight;
// Iterate through each point in the geometry.
//
for (; !iter.isDone(); iter.next()) {
MPoint pt = iter.position();
MPoint skinned;
rBlend = MQuaternion(); // reset
tBlend = MQuaternion(); // reset
rBlend[3] = 0.0;
tBlend[3] = 0.0;
// get the weights for this point
MArrayDataHandle weightsHandle = weightListHandle.inputValue().child(weights);
// compute the skinning
for (int i = 0; i<numTransforms; ++i) {
if (MS::kSuccess == weightsHandle.jumpToElement(i)) {
weight = weightsHandle.inputValue().asDouble();
scaleMe = rQuaternions.at(i);
rBlend = rBlend + scaleMe.scaleIt(weight);
scaleMe = tQuaternions.at(i);
tBlend = tBlend + scaleMe.scaleIt(weight);
}
}
MMatrix dqMatrix = makeDQMatrix(rBlend.normalizeIt(), tBlend);
skinned = pt * dqMatrix;
// Set the final position.
iter.setPosition(skinned);
// advance the weight list handle
weightListHandle.next();
}
return returnStatus;
}
void MScene::updatePhysics(void)
{
MPhysicsContext * physics = MEngine::getInstance()->getPhysicsContext();
if(! physics)
return;
unsigned int i;
unsigned int size = getEntitiesNumber();
for(i=0; i<size; i++)
{
MOEntity * entity = getEntityByIndex(i);
if(! entity->isActive())
continue;
MPhysicsProperties * phyProps = entity->getPhysicsProperties();
if(! phyProps)
continue;
if(phyProps->getCollisionObjectId() > 0)
{
MObject3d * parent = entity->getParent();
if(parent && phyProps->isGhost())
{
MVector3 euler = entity->getTransformedRotation();
physics->setObjectTransform(
phyProps->getCollisionObjectId(),
entity->getTransformedPosition(),
MQuaternion(euler.x, euler.y, euler.z)
);
}
else if(entity->needToUpdate())
{
physics->setObjectTransform(
phyProps->getCollisionObjectId(),
entity->getPosition(),
entity->getRotation()
);
}
}
}
physics->setWorldGravity(m_gravity);
physics->updateSimulation();
for(i=0; i<size; i++)
{
MOEntity * entity = getEntityByIndex(i);
if(! entity->isActive())
continue;
MPhysicsProperties * phyProps = entity->getPhysicsProperties();
if(phyProps)
{
if((phyProps->getCollisionObjectId() > 0) && (! phyProps->isGhost()))
{
MVector3 position = entity->getPosition();
MQuaternion rotation = entity->getRotation();
physics->getObjectTransform(phyProps->getCollisionObjectId(), &position, &rotation);
entity->setPosition(position);
entity->setRotation(rotation);
}
}
}
}
void MScene::prepareCollisionObject(MOEntity * entity)
{
MPhysicsContext * physics = MEngine::getInstance()->getPhysicsContext();
MPhysicsProperties * phyProps = entity->getPhysicsProperties();
if(! phyProps)
return;
unsigned int shapeId = phyProps->getShapeId();
if(shapeId == 0)
return;
// has physics parent
MPhysicsProperties * parentPhyProps = NULL;
MOEntity * parentEntity = NULL;
if(! phyProps->isGhost())
{
MObject3d * parentObject = entity->getParent();
if(parentObject)
{
if(parentObject->getType() == M_OBJECT3D_ENTITY)
{
parentEntity = (MOEntity*)parentObject;
parentPhyProps = parentEntity->getPhysicsProperties();
}
}
}
if(parentPhyProps) // add shape to parent multi-shape
{
MVector3 position = entity->getPosition() * parentEntity->getTransformedScale();
MQuaternion rotation = entity->getRotation();
phyProps->setShapeId(shapeId);
physics->addChildShape(parentPhyProps->getShapeId(), shapeId, position, rotation);
}
else // create collision object
{
unsigned int collisionObjectId;
if(phyProps->isGhost())
{
MVector3 euler = entity->getTransformedRotation();
physics->createGhost(
&collisionObjectId, shapeId,
entity->getTransformedPosition(),
MQuaternion(euler.x, euler.y, euler.z)
);
phyProps->setShapeId(shapeId);
phyProps->setCollisionObjectId(collisionObjectId);
}
else
{
physics->createRigidBody(
&collisionObjectId, shapeId,
entity->getPosition(), entity->getRotation(),
phyProps->getMass()
);
phyProps->setShapeId(shapeId);
phyProps->setCollisionObjectId(collisionObjectId);
physics->setObjectRestitution(collisionObjectId, phyProps->getRestitution());
physics->setObjectDamping(collisionObjectId, phyProps->getLinearDamping(), phyProps->getAngularDamping());
physics->setObjectAngularFactor(collisionObjectId, phyProps->getAngularFactor());
physics->setObjectLinearFactor(collisionObjectId, *phyProps->getLinearFactor());
physics->setObjectFriction(collisionObjectId, phyProps->getFriction());
}
// deactivate
if(! entity->isActive())
physics->deactivateObject(collisionObjectId);
// set user pointer (entity)
physics->setObjectUserPointer(collisionObjectId, entity);
}
}
bool createShape(MOEntity * entity, MPhysicsProperties * phyProps, unsigned int * shapeId)
{
MPhysicsContext * physics = MEngine::getInstance()->getPhysicsContext();
// get bounding box
MBox3d * box = entity->getBoundingBox();
MVector3 scale = entity->getTransformedScale();
// swith shapes
switch(phyProps->getCollisionShape())
{
default:
case M_COLLISION_SHAPE_BOX:
physics->createBoxShape(shapeId, (box->max - box->min)*scale*0.5f);
break;
case M_COLLISION_SHAPE_SPHERE:
{
MVector3 vec = (box->max - box->min)*scale*0.5f;
float radius = vec.x;
radius = MAX(radius, vec.y);
radius = MAX(radius, vec.z);
physics->createSphereShape(shapeId, radius);
}
break;
case M_COLLISION_SHAPE_CONE:
{
MVector3 vec = (box->max - box->min)*scale;
float height = vec.y;
float radius = vec.x*0.5f;
radius = MAX(radius, vec.z*0.5f);
physics->createConeShape(shapeId, radius, height);
}
break;
case M_COLLISION_SHAPE_CAPSULE:
{
MVector3 vec = (box->max - box->min)*scale;
float height = vec.y;
float radius = vec.x*0.5f;
radius = MAX(radius, vec.z*0.5f);
physics->createCylinderShape(shapeId, radius, height);
}
break;
case M_COLLISION_SHAPE_CYLINDER:
{
MVector3 vec = (box->max - box->min)*scale;
float height = vec.y;
float radius = vec.x*0.5f;
radius = MAX(radius, vec.z*0.5f);
physics->createCylinderShape(shapeId, radius, height);
}
break;
case M_COLLISION_SHAPE_CONVEX_HULL:
{
MMesh * mesh = entity->getMesh();
if(mesh)
{
MSubMesh * subMeshs = mesh->getSubMeshs();
unsigned int subMeshsNumber = mesh->getSubMeshsNumber();
if(subMeshsNumber == 0)
return false;
if(subMeshsNumber == 1)
{
MSubMesh * subMesh = &subMeshs[0];
if(subMesh->getVerticesSize() > 0)
physics->createConvexHullShape(shapeId, subMesh->getVertices(), subMesh->getVerticesSize(), entity->getScale());
}
else
{
physics->createMultiShape(shapeId);
unsigned int s;
for(s=0; s<subMeshsNumber; s++)
{
unsigned int subShapeId;
MSubMesh * subMesh = &subMeshs[s];
if(subMesh->getVerticesSize() > 0)
{
physics->createConvexHullShape(&subShapeId, subMesh->getVertices(), subMesh->getVerticesSize(), entity->getScale());
physics->addChildShape(*shapeId, subShapeId, MVector3(), MQuaternion());
}
}
}
}
else{
return false;
}
}
break;
case M_COLLISION_SHAPE_TRIANGLE_MESH:
{
MMesh * mesh = entity->getMesh();
if(mesh)
{
MSubMesh * subMeshs = mesh->getSubMeshs();
unsigned int subMeshsNumber = mesh->getSubMeshsNumber();
if(subMeshsNumber == 0)
return false;
if(subMeshsNumber == 1)
{
MSubMesh * subMesh = &subMeshs[0];
if(subMesh->getVerticesSize() >= 3)
physics->createTriangleMeshShape(shapeId,
subMesh->getVertices(), subMesh->getVerticesSize(),
subMesh->getIndices(), subMesh->getIndicesSize(), subMesh->getIndicesType(),
entity->getScale()
);
}
else
{
physics->createMultiShape(shapeId);
unsigned int s;
for(s=0; s<subMeshsNumber; s++)
{
unsigned int subShapeId;
MSubMesh * subMesh = &subMeshs[s];
if(subMesh->getVerticesSize() >= 3)
{
physics->createTriangleMeshShape(&subShapeId,
subMesh->getVertices(), subMesh->getVerticesSize(),
subMesh->getIndices(), subMesh->getIndicesSize(), subMesh->getIndicesType(),
entity->getScale()
);
physics->addChildShape(*shapeId, subShapeId, MVector3(), MQuaternion());
}
}
}
}
else{
return false;
}
}
break;
}
return true;
}
//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);
}
}
}
}
// COMPUTE ======================================
MStatus gear_rollSplineKine::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != output)
return MS::kUnknownParameter;
// Get inputs matrices ------------------------------
// Inputs Parent
MArrayDataHandle adh = data.inputArrayValue( ctlParent );
int count = adh.elementCount();
if (count < 1)
return MS::kFailure;
MMatrixArray inputsP(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputsP[i] = adh.inputValue().asMatrix();
}
// Inputs
adh = data.inputArrayValue( inputs );
if (count != adh.elementCount())
return MS::kFailure;
MMatrixArray inputs(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputs[i] = adh.inputValue().asMatrix();
}
adh = data.inputArrayValue( inputsRoll );
if (count != adh.elementCount())
return MS::kFailure;
MDoubleArray roll(adh.elementCount());
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
roll[i] = degrees2radians((double)adh.inputValue().asFloat());
}
// Output Parent
MDataHandle ha = data.inputValue( outputParent );
MMatrix outputParent = ha.asMatrix();
// Get inputs sliders -------------------------------
double in_u = (double)data.inputValue( u ).asFloat();
bool in_resample = data.inputValue( resample ).asBool();
int in_subdiv = data.inputValue( subdiv ).asShort();
bool in_absolute = data.inputValue( absolute ).asBool();
// Process ------------------------------------------
// Get roll, pos, tan, rot, scl
MVectorArray pos(count);
MVectorArray tan(count);
MQuaternion *rot;
rot = new MQuaternion[count];
MVectorArray scl(count);
double threeDoubles[3];
for (int i = 0 ; i < count ; i++){
MTransformationMatrix tp(inputsP[i]);
MTransformationMatrix t(inputs[i]);
pos[i] = t.getTranslation(MSpace::kWorld);
rot[i] = tp.rotation();
t.getScale(threeDoubles, MSpace::kWorld);
scl[i] = MVector(threeDoubles[0], threeDoubles[1], threeDoubles[2]);
tan[i] = MVector(threeDoubles[0] * 2.5, 0, 0).rotateBy(t.rotation());
}
// Get step and indexes
// We define between wich controlers the object is to be able to
// calculate the bezier 4 points front this 2 objects
double step = 1.0 / max( 1, count-1.0 );
int index1 = (int)min( count-2.0, in_u/step );
int index2 = index1+1;
int index1temp = index1;
int index2temp = index2;
double v = (in_u - step * double(index1)) / step;
double vtemp = v;
// calculate the bezier
MVector bezierPos;
MVector xAxis, yAxis, zAxis;
if(!in_resample){
// straight bezier solve
MVectorArray results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
bezierPos = results[0];
xAxis = results[1];
}
else if(!in_absolute){
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[index1];
presampletan[0] = tan[index1];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results(2);
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],sampleu);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(v>=samplelen[i] && v <= samplelen[i+1]){
v = (v - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],v);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],v);
break;
}
}
}
else{
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[0];
presampletan[0] = tan[0];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results;
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
index1 = (int)min(count-2,sampleu / step);
index2 = index1+1;
v = (sampleu - step * double(index1)) / step;
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(in_u>=samplelen[i] && in_u <= samplelen[i+1]){
in_u = (in_u - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],in_u);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],in_u);
break;
}
}
}
// compute the scaling (straight interpolation!)
MVector scl1 = linearInterpolate(scl[index1temp], scl[index2temp],vtemp);
// compute the rotation!
MQuaternion q = slerp(rot[index1temp], rot[index2temp], vtemp);
yAxis = MVector(0,1,0);
yAxis = yAxis.rotateBy(q);
// use directly or project the roll values!
// print roll
double a = linearInterpolate(roll[index1temp], roll[index2temp], vtemp);
yAxis = yAxis.rotateBy( MQuaternion(xAxis.x * sin(a/2.0), xAxis.y * sin(a/2.0), xAxis.z * sin(a/2.0), cos(a/2.0)));
zAxis = xAxis ^ yAxis;
zAxis.normalize();
yAxis = zAxis ^ xAxis;
yAxis.normalize();
// Output -------------------------------------------
MTransformationMatrix result;
// translation
result.setTranslation(bezierPos, MSpace::kWorld);
// rotation
q = getQuaternionFromAxes(xAxis,yAxis,zAxis);
result.setRotationQuaternion(q.x, q.y, q.z, q.w);
// scaling
threeDoubles[0] = 1;
threeDoubles[0] = scl1.y;
threeDoubles[0] = scl1.z;
result.setScale(threeDoubles, MSpace::kWorld);
MDataHandle h = data.outputValue( output );
h.setMMatrix( result.asMatrix() * outputParent.inverse() );
data.setClean( plug );
return MS::kSuccess;
}