const Matrix Matrix::rotationOntoXAxis(const Vector &v, Matrix *inverse)
{
Vector xAxis(1, 0, 0), yAxis(0, 1, 0);
Vector u = v.normalized();
// Project u onto the yz plane
Vector uProj(0, u.y, u.z);
uProj.normalize();
// Get the sin and cos of the angle between uProj and the y axis (A)
double cosA = uProj.dot(yAxis);
double sinA = uProj.cross(yAxis).length();
// Build a matrix that rotates around the x axis with angle A, thereby
// rotating u into the xy plane. We need clockwise rotation if u.z is
// positive, counterclockwise rotation otherwise.
if (u.z > 0) {
// Clockwise rotation: counterclockwise rotation with angle -A
// cos(-A) = cos(A) but sin(-A) = -sin(A)
sinA = -sinA;
}
Matrix rotA(Vector(1, 0, 0), Vector(0, cosA, -sinA), Vector(0, sinA, cosA));
// Rotate u
u = (rotA*u).normalized();
// Get the sin and cos of the angle between u and the x axis (B)
double cosB = u.dot(xAxis);
double sinB = u.cross(xAxis).length();
// Build a matrix that rotates around the z axis with angle B, thereby
// rotating u onto the x axis. We need clockwise rotation if u.y is
// positive, counterclockwise rotation otherwise.
if (u.y > 0) {
// Angle negation trick, see above
sinB = -sinB;
}
Matrix rotB(Vector(cosB, -sinB, 0), Vector(sinB, cosB, 0), Vector(0, 0, 1));
// Compute inverse if requested
if (inverse) {
Matrix invRotA(Vector(1, 0, 0), Vector(0, cosA, sinA), Vector(0, -sinA, cosA));
Matrix invRotB(Vector(cosB, sinB, 0), Vector(-sinB, cosB, 0), Vector(0, 0, 1));
*inverse = invRotA*invRotB;
}
return rotB*rotA;
}
//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);
}
