Transform trSetTransform(const float4& translation, const Quaternion& quat)
{
    Transform tr;
    tr.m_translation = translation;
    tr.m_rotation = qtGetRotationMatrix( quat );
    return tr;
}
Esempio n. 2
0
__inline void drawPointListTransformed(const float4* vtx,  int nVtx, const float4& translation, const Quaternion& quat)
{
    glPushMatrix();

    Matrix3x3 rotMat = mtTranspose( qtGetRotationMatrix( quat ) );
    float transformMat[16] =
    {
        rotMat.m_row[0].x, rotMat.m_row[0].y, rotMat.m_row[0].z, 0,
        rotMat.m_row[1].x, rotMat.m_row[1].y, rotMat.m_row[1].z, 0,
        rotMat.m_row[2].x, rotMat.m_row[2].y, rotMat.m_row[2].z, 0,
        translation.x, translation.y, translation.z,1
    };

    glMultMatrixf( transformMat );

    float4 c = make_float4(1,1,0,0);

    glPointSize(3.f);
    glBegin(GL_POINTS);
    for(int i=0; i<nVtx; i++)
    {
        glColor4f(c.x, c.y, c.z, 1);
        glVertexFloat4( vtx[i] );
    }
    glEnd();

    glPopMatrix();
}
int btGpuNarrowphaseAndSolver::registerRigidBody(int shapeIndex, float mass, const float* position, const float* orientation , bool writeToGpu)
{
	assert(m_internalData->m_numAcceleratedRigidBodies< (MAX_CONVEX_BODIES_CL-1));
		
	m_internalData->m_bodyBufferGPU->resize(m_internalData->m_numAcceleratedRigidBodies+1);

	RigidBodyBase::Body& body = m_internalData->m_bodyBufferCPU->at(m_internalData->m_numAcceleratedRigidBodies);

	float friction = 1.f;
	float restitution = 0.f;

	body.m_frictionCoeff = friction;
	body.m_restituitionCoeff = restitution;
	body.m_angVel = make_float4(0.f);
	body.m_linVel = make_float4(0.f);
	body.m_pos = make_float4(position[0],position[1],position[2],0.f);
	body.m_quat = make_float4(orientation[0],orientation[1],orientation[2],orientation[3]);
	body.m_shapeIdx = shapeIndex;
	if (shapeIndex<0)
	{
		body.m_shapeType = CollisionShape::SHAPE_PLANE;
		m_planeBodyIndex = m_internalData->m_numAcceleratedRigidBodies;
	} else
	{
		body.m_shapeType = CollisionShape::SHAPE_CONVEX_HEIGHT_FIELD;
	}
	
	body.m_invMass = mass? 1.f/mass : 0.f;

	if (writeToGpu)
	{
		m_internalData->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_internalData->m_numAcceleratedRigidBodies);
	}

	RigidBodyBase::Inertia& shapeInfo = m_internalData->m_inertiaBufferCPU->at(m_internalData->m_numAcceleratedRigidBodies);

	if (mass==0.f)
	{
		shapeInfo.m_initInvInertia = mtZero();
		shapeInfo.m_invInertia = mtZero();
	} else
	{

		assert(body.m_shapeIdx>=0);

		//approximate using the aabb of the shape

		Aabb aabb = (*m_internalData->m_shapePointers)[shapeIndex]->m_aabb;
		float4 halfExtents = (aabb.m_max - aabb.m_min);

		float4 localInertia;

		float lx=2.f*halfExtents.x;
		float ly=2.f*halfExtents.y;
		float lz=2.f*halfExtents.z;

		localInertia = make_float4( (mass/12.0f) * (ly*ly + lz*lz),
			(mass/12.0f) * (lx*lx + lz*lz),
			(mass/12.0f) * (lx*lx + ly*ly));

		float4 invLocalInertia;
		invLocalInertia.x = 1.f/localInertia.x;
		invLocalInertia.y = 1.f/localInertia.y;
		invLocalInertia.z = 1.f/localInertia.z;
		invLocalInertia.w = 0.f;

		shapeInfo.m_initInvInertia = mtZero();
		shapeInfo.m_initInvInertia.m_row[0].x = invLocalInertia.x;
		shapeInfo.m_initInvInertia.m_row[1].y = invLocalInertia.y;
		shapeInfo.m_initInvInertia.m_row[2].z = invLocalInertia.z;

		Matrix3x3 m = qtGetRotationMatrix( body.m_quat);
		Matrix3x3 mT = mtTranspose( m );
		shapeInfo.m_invInertia = mtMul( mtMul( m, shapeInfo.m_initInvInertia ), mT );

	}

	if (writeToGpu)
		m_internalData->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_internalData->m_numAcceleratedRigidBodies);



	return m_internalData->m_numAcceleratedRigidBodies++;
}