Beispiel #1
0
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;
	}
}
Beispiel #2
0
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])
	);
}
Beispiel #3
0
void MObject3d::setAxisAngleRotation(const MVector3 & axis, const float angle)
{
	MQuaternion rotation = MQuaternion(angle, axis);
	if(rotation != m_rotation)
	{
		m_rotation = rotation;
		m_needToUpdate = true;
	}
}
Beispiel #4
0
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);
	}
}
Beispiel #5
0
MQuaternion convert( const Imath::Quatd &from )
{
	return MQuaternion( from[1], from[2], from[3], from[0] );
}
Beispiel #6
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);
}
Beispiel #8
0
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;
}
Beispiel #9
0
void MObject3d::addAxisAngleRotation(const MVector3 & axis, const float angle)
{
    M_PROFILE_SCOPE(MObject3d::addAxisAngleRotation);
	m_rotation *= MQuaternion(angle, axis);
	m_needToUpdate = true;
}
Beispiel #10
0
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]));
}
Beispiel #11
0
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;
}
Beispiel #12
0
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);
			}
		}
	}
}
Beispiel #13
0
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);
	}
}
Beispiel #14
0
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;
}
Beispiel #15
0
//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);
            }
        }
    }
}
Beispiel #16
0
// 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;
}