btGeneric6DofConstraint : rotation of local transforms ?

[stevet]

Hello everyone.
Let me first thank the authors for this great engine that I'm currently
discovering, and then apologize for the long post.

I'm trying to create a chain of constrained rigid bodies, given a definition
inputed in a text file.

For instance if my definition file looks like this:

Code: Select all

ROOT ID 0 
OFFSET 0 1 0

JOINT ID 1
PARENT ID 0
OFFSET -1 0 0
MINANGLES 0 0 0
MAXANGLES 0 0 0

JOINT ID 2
PARENT ID 1
OFFSET 1 1 0
MINANGLES 0 0 0
MAXANGLES 0 0 0

As a result I obtain something like this :

good.png

Each joint except for the root has a parent, and an offset from this parent that defines
the length of the next rigid body to create. MINANGLES and MAXANGLES determine, as their name suggest,
the angular limits of a btGeneric6DofConstraint I want to create.

The code that follows successfully creates the rigid bodies, but my problems appear when I create the constraints.

I would like to set the rotations of the constraints so that the initial world position and orientation of each
rigid body matches with initial angle values equal to 0 for X, Y and Z.

Working code:

Code: Select all

void BuildOneJoint2(const btTransform& currentTransform, const btVector3& origin, const joint_def_t& joint, btDynamicsWorld* m_ownerWorld, btCollisionShape** m_shapes, btRigidBody** m_bodies, btTypedConstraint** m_joints, unsigned int& id, int parentId)
	{
		const btVector3 vUp(0,1,0);
		const btVector3 vForward(1,0,0);
		const btVector3 vBack(0,0,1);

		// retrieving joint offset from parent
                // definitions from text files are contained in struct joint 
		btVector3 offSetVector(btScalar(joint.offset[0]), btScalar(joint.offset[1]),  btScalar(joint.offset[2]));
		btVector3 parentOffset;

		//computing radius and length of capsule; radius is more or less equal to offset
		btScalar radius = offSetVector.length() * btScalar(0.1); 
		btScalar length = offSetVector.length() - 2 * radius;
		if(parentId == 0)
		{
			// if current joint is root, for now juste create a small capsule representing root
			btVector3 rootOffSet(offSetVector); rootOffSet.normalize();
			parentOffset = rootOffSet * btScalar(0.05);
			offSetVector += parentOffset;
		}
		else
		{
			parentOffset = btVector3(btScalar(joint.parent->offset[0]), btScalar(joint.parent->offset[1]),  btScalar(joint.parent->offset[2]));
		}
		// Setup the geometry
		//The total height of a btCapsuleShape is height+2*radius
		m_shapes[id] = new btCapsuleShapeX(radius, length - btScalar(0.1));
		
		// Setup the rigid body
		btTransform offset;
		offset.setIdentity();
  		offset.setOrigin(origin + offSetVector / 2);
		
		// computing rotation necessary for capsule
		btVector3 vToBone(offSetVector);
		vToBone.normalize();
		btVector3 vAxis = vToBone.cross(vForward);
		btScalar dot = vToBone.dot(vForward);
		btScalar angle = acos(vToBone.dot(vForward));
		if(angle != 0)
		{
			offset.setRotation(btQuaternion(vBack, angle));
		}

		m_bodies[id] = localCreateRigidBody(m_ownerWorld, btScalar(1.), offset, m_shapes[id]);
		
		btVector3 newOrigin(origin + offSetVector);

		btTransform newTransform; newTransform.setIdentity();
		newTransform.setRotation(currentTransform.getRotation() * offset.getRotation());
		newTransform.setOrigin(newOrigin);
		int newParentId = id;
		// method is recursive, call it with children
		for(unsigned int i=0; i< joint.nbChildren_; ++i)
		{
  			BuildOneJoint2(newTransform, newOrigin, *(joint.children[i]), m_ownerWorld, m_shapes, m_bodies, m_joints, ++id, newParentId);
		}
	}

The code for

Code: Select all

 localCreateRigidBody 

is taken from the demos :

Code: Select all

btRigidBody* localCreateRigidBody (btDynamicsWorld* m_ownerWorld, btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
	{
		btVector3 localInertia(0,0,0);
		if (mass != 0.f) // is dynamic
			shape->calculateLocalInertia(mass,localInertia);

		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
		
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		m_ownerWorld->addRigidBody(body);
		return body;
	}

Considering the previous code, I now try to create my constraints:

Code: Select all

btGeneric6DofConstraint* constraint;

btTransform localA; localA.setIdentity();
//localA.setRotation(???);
localA.setOrigin(btVector3(parentOffset.length() / 2, btScalar(0.), btScalar(0.)));

btTransform localB; localB.setIdentity();
//localB.setRotation(???);
localB.setOrigin(btVector3(-length / 2, btScalar(0.), btScalar(0.)));

constraint =  new btGeneric6DofConstraint(*m_bodies[parentId], *m_bodies[id], localA, localB, false);
m_joints[id] = constraint;
constraint->setDbgDrawSize(0.5f);

btVector3 limitsDown(btScalar(joint.minAngleValues[0] * DegreesToRadians), 
                                btScalar(joint.minAngleValues[1] * DegreesToRadians), 
                                btScalar(joint.minAngleValues[2] * DegreesToRadians));

btVector3 limitsUp(btScalar(joint.maxAngleValues[0] * DegreesToRadians), 
                            btScalar(joint.maxAngleValues[1] * DegreesToRadians), 
                            btScalar(joint.maxAngleValues[2] * DegreesToRadians));

constraint->setAngularLowerLimit(limitsDown);
constraint->setAngularUpperLimit(limitsUp);

m_ownerWorld->addConstraint(m_joints[id], true);

As I said, I would like to set the rotations of the constraints so that the initial world position and orientation of each
rigid body matches with initial angle values equal to 0 for X, Y and Z for each constraint.

I've tried several possibilities without achieving the desired goal, and adding constraints gives me something like that in the end:

bad.png

It is not really easy to write on a forum, and I hope I am clear enough.
Thank you very much for your time,

Steve