Add 1D and 2D support for Bullet: using rigidbody->angularFactor(const btVector3& factor) and body->setLinearFactor(const btVector3& linearFactor);

For example, to only allow linear motion in the X-Z plane, and only rotation around Y axis use: body->setLinearFactor(btVector3(1,0,1)); body->setAngularFactor(btVector3(0,1,0)); Fix build issues with CodeBlocks, when generating projectfiles using CMake 2.6: ${OPENGL_glU_LIBRARY} should be ${OPENGL_glu_LIBRARY} Fix build issue with CodeBlocks, comment out xmlfree in Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp (will leak memory)
2009-03-08 04:14:17 +00:00
parent 3317f17270
commit 700db838b1
8 changed files with 91 additions and 77 deletions
--- a/Demos/ForkLiftDemo/CMakeLists.txt
+++ b/Demos/ForkLiftDemo/CMakeLists.txt
@@ -52,7 +52,7 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL }
 )

 LINK_LIBRARIES(
-OpenGLSupport BulletDynamics  BulletCollision LinearMath    ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glU_LIBRARY}
+OpenGLSupport BulletDynamics  BulletCollision LinearMath    ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
 )

 ADD_EXECUTABLE(AppForkLiftDemo
--- a/Demos/OpenGL/DemoApplication.cpp
+++ b/Demos/OpenGL/DemoApplication.cpp
@@ -71,7 +71,7 @@ m_cameraUp(0,1,0),
 m_forwardAxis(2),	
 m_glutScreenWidth(0),
 m_glutScreenHeight(0),
-m_ShootBoxInitialSpeed(40.f),
+m_ShootBoxInitialSpeed(4.f),
 m_stepping(true),
 m_singleStep(false),
 m_idle(false),
@@ -544,7 +544,7 @@ void	DemoApplication::setShootBoxShape ()
 			btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f));
 			m_shootBoxShape = new btUniformScalingShape(childShape,0.5f);
 #else
-			m_shootBoxShape = new btSphereShape(1.f);//BoxShape(btVector3(1.f,1.f,1.f));
+			m_shootBoxShape = new btSphereShape(.1f);//BoxShape(btVector3(1.f,1.f,1.f));
 #endif//
 		}
 }
@@ -554,7 +554,7 @@ void	DemoApplication::shootBox(const btVector3& destination)

 	if (m_dynamicsWorld)
 	{
-		float mass = 10.f;
+		float mass = 0.1f;
 		btTransform startTransform;
 		startTransform.setIdentity();
 		btVector3 camPos = getCameraPosition();
@@ -563,6 +563,7 @@ void	DemoApplication::shootBox(const btVector3& destination)
 		setShootBoxShape ();

 		btRigidBody* body = this->localCreateRigidBody(mass, startTransform,m_shootBoxShape);
+		body->setLinearFactor(btVector3(1,1,1));

 		btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);
 		linVel.normalize();
--- a/Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp
+++ b/Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp
@@ -1,14 +1,14 @@
 /*
 * Copyright 2006 Sony Computer Entertainment Inc.
 *
- * Licensed under the SCEA Shared Source License, Version 1.0 (the "License"); you may not use this 
+ * Licensed under the SCEA Shared Source License, Version 1.0 (the "License"); you may not use this
 * file except in compliance with the License. You may obtain a copy of the License at:
 * http://research.scea.com/scea_shared_source_license.html
 *
- * Unless required by applicable law or agreed to in writing, software distributed under the License 
- * is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 
- * implied. See the License for the specific language governing permissions and limitations under the 
- * License. 
+ * Unless required by applicable law or agreed to in writing, software distributed under the License
+ * is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ * implied. See the License for the specific language governing permissions and limitations under the
+ * License.
 */

 // This is a rework of the XML plugin that contains a complete interface to libxml2 "readXML"
@@ -77,8 +77,8 @@ void daeLIBXMLPlugin::getProgress(daeInt* bytesParsed,
 daeInt daeLIBXMLPlugin::read(daeURI& uri, daeString docBuffer)
 {
    // Make sure topMeta has been set before proceeding
-	
-	if (topMeta == NULL) 
+
+	if (topMeta == NULL)
 	{
 		return DAE_ERR_BACKEND_IO;
 	}
@@ -127,7 +127,7 @@ daeInt daeLIBXMLPlugin::read(daeURI& uri, daeString docBuffer)
 	daeElementRef domObject = startParse(topMeta, reader);

 	// Parsing done, free the xmlReader and error check to make sure we got a valid DOM back
-	
+
 	xmlFreeTextReader(reader);

 	if (!domObject)
@@ -149,11 +149,11 @@ daeInt daeLIBXMLPlugin::read(daeURI& uri, daeString docBuffer)

 	// Make a vector to store a list of the integration items that need to be processed later
 	// postProcessDom will fill this in for us (this should probably not be done in the IOPlugin)
-	
+
 	std::vector<INTEGRATION_ITEM> intItems;
-	
+
 	//insert the elements into the database, for this DB the elements are the Collada object which have
-	//an ID. 
+	//an ID.
 	//this function will fill the _integrationItems array as well
 	postProcessDom(document, domObject, intItems);
 	database->validate();
@@ -164,7 +164,7 @@ daeInt daeLIBXMLPlugin::read(daeURI& uri, daeString docBuffer)
 	int i;
 	for (i=0;i<size;i++)
 		intItems[i].intObject->createFromChecked(intItems[i].element);
-	
+
 	for (i=0;i<size;i++)
 		intItems[i].intObject->fromCOLLADAChecked();

@@ -238,7 +238,7 @@ daeLIBXMLPlugin::startParse(daeMetaElement* thisMetaElement, xmlTextReaderPtr re
 	}
 #endif

-	
+
 	ret = xmlTextReaderRead(reader);
 	// If we're out of data, return the element
 	if(ret != 1)
@@ -308,11 +308,11 @@ void daeLIBXMLPlugin::readAttributes( daeElement *element, xmlTextReaderPtr read
 		while(xmlTextReaderMoveToNextAttribute(reader))
 		{
 			daeMetaAttribute *ma = element->getMeta()->getMetaAttribute((const daeString)xmlTextReaderConstName(reader));
-			if( ( ma != NULL && ma->getType() != NULL && ma->getType()->getUsesStrings() ) || 
+			if( ( ma != NULL && ma->getType() != NULL && ma->getType()->getUsesStrings() ) ||
 				strcmp(element->getMeta()->getName(), "any") == 0 )
 			{
 				// String is used as one piece
-				if(!element->setAttribute( (const daeString)xmlTextReaderConstName(reader), 
+				if(!element->setAttribute( (const daeString)xmlTextReaderConstName(reader),
 					(const daeString)xmlTextReaderConstValue(reader) ) )
 				{
 					const xmlChar * attName	 = xmlTextReaderConstName(reader);
@@ -339,7 +339,7 @@ void daeLIBXMLPlugin::readAttributes( daeElement *element, xmlTextReaderPtr read
 					memset( err, 0, 512 );
 #if LIBXML_VERSION >= 20620
 					sprintf(err,"The DOM was unable to create an attribute %s = %s at line %d\nProbably a schema violation.\n", attName, attValue ,xmlTextReaderGetParserLineNumber(reader));
-#else				
+#else
 					sprintf(err,"The DOM was unable to create an attribute %s = %s \nProbably a schema violation.\n", attName, attValue);
 #endif
 					daeErrorHandler::get()->handleWarning( err );
@@ -377,7 +377,8 @@ void daeLIBXMLPlugin::readAttributes( daeElement *element, xmlTextReaderPtr read
 						}
 					}
 				}
-				xmlFree(value);
+				//rather leak than break compilers (CodeBlocks/MinGW)
+				//xmlFree(value);
 			}
 		}
 	}
@@ -425,7 +426,8 @@ void daeLIBXMLPlugin::readValue( daeElement *element, xmlTextReaderPtr reader )
 				// eat the characters we just read (would be nice if set returned characters used.
 			}
 		}
-		xmlFree(value);
+		//rather leak than break compilers (CodeBlocks/MinGW)
+		//xmlFree(value);
 	}
 	int ret = xmlTextReaderRead(reader);
 	assert(ret==1);
@@ -466,7 +468,7 @@ daeLIBXMLPlugin::nextElement(daeMetaElement* thisMetaElement, xmlTextReaderPtr r

 	//try and read attributes
 	readAttributes( element, reader );
-	
+
 	ret = xmlTextReaderRead(reader);
 	// If we're out of data, return the element
 	if(ret != 1)
@@ -550,7 +552,7 @@ void daeLIBXMLPlugin::postProcessDom(daeDocument *document, daeElement* element,
 	for ( size_t x = 0; x < children.getCount(); x++ ) {
 		postProcessDom(document, children.get(x), intItems);
 	}
-	
+
 	/*if (element->getMeta()->getContents() != NULL) {
 		daeMetaElementArrayAttribute *contents = element->getMeta()->getContents();
 		for ( int i = 0; i < contents->getCount( element ); i++ ) {
@@ -618,9 +620,9 @@ daeInt daeLIBXMLPlugin::write(daeURI *name, daeDocument *document, daeBool repla
 	xmlTextWriterSetIndentString( writer, indentString );
 	xmlTextWriterSetIndent( writer, 1 );
 	xmlTextWriterStartDocument( writer, NULL, NULL, NULL );
-	
+
 	writeElement( document->getDomRoot() );
-	
+
 	xmlTextWriterEndDocument( writer );
 	xmlTextWriterFlush( writer );
 	xmlFreeTextWriter( writer );
@@ -645,9 +647,9 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
            xmlTextWriterStartElement(writer, (xmlChar*)(daeString)_meta->getName());
 		}
 		daeMetaAttributeRefArray& attrs = _meta->getMetaAttributes();
-		
+
 		int acnt = (int)attrs.getCount();
-		
+
 		for(int i=0;i<acnt;i++) {
 			writeAttribute( attrs[i], element, i );
 		}
@@ -655,7 +657,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 	daeMetaAttribute* valueAttr = _meta->getValueAttribute();
 	if (valueAttr!=NULL)
 		writeAttribute(valueAttr, element);
-	
+
 	daeElementRefArray children;
 	element->getChildren( children );
 	for ( size_t x = 0; x < children.getCount(); x++ ) {
@@ -695,7 +697,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 /*void daeLIBXMLPlugin::writeAttribute( daeMetaAttribute* attr, daeElement* element, daeInt attrNum )
 {
 	static daeChar atomicTypeBuf[TYPE_BUFFER_SIZE];
-	
+
 	if (element == NULL)
 		return;
 	if ( attr->getCount(element) == 0 ) {
@@ -706,7 +708,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 		}
 		return;
 	}
-	else if ( attr->getCount(element) == 1 ) { 
+	else if ( attr->getCount(element) == 1 ) {
 		//single value or an array of a single value
 		char* elemMem = attr->get(element, 0);

@@ -718,7 +720,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 			if(attr->getDefault() != NULL)
 			{
 				#if 1
-				// The attribute has a default, convert the default to binary and suppress 
+				// The attribute has a default, convert the default to binary and suppress
 				// output of the attribute if the value matches the default.
 				// DISABLE THIS CODE IF YOU WANT DEFAULT VALUES TO ALWAYS EXPORT
 				if(typeSize >= TYPE_BUFFER_SIZE)
@@ -748,7 +750,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 					if(elemMem[i] != 0)
 						break;
 				}
-				if(i == typeSize && attr->getContainer()->getValueAttribute() != attr && 
+				if(i == typeSize && attr->getContainer()->getValueAttribute() != attr &&
 					attr->getType()->getTypeEnum() != daeAtomicType::BoolType &&
 					attr->getType()->getTypeEnum() != daeAtomicType::EnumType )
 					return;
@@ -768,7 +770,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 					(daeString)attr->getName(),(daeString)attr->getContainer()->getName());
 			daeErrorHandler::get()->handleError( msg );
 		}
-					
+
 		// Suppress attributes that convert to an empty string.

 		if (strlen(atomicTypeBuf) == 0)
@@ -804,7 +806,7 @@ void daeLIBXMLPlugin::writeElement( daeElement* element )
 		}
 		for( int i = 0; i < attr->getCount(element); i++ ) {
 			char* elemMem = attr->get(element, i);
-			if (attr->getType()->memoryToString(elemMem, atomicTypeBuf,	TYPE_BUFFER_SIZE)== false) 
+			if (attr->getType()->memoryToString(elemMem, atomicTypeBuf,	TYPE_BUFFER_SIZE)== false)
 			{
 				char msg[512];
 				sprintf(msg,
@@ -842,7 +844,7 @@ void daeLIBXMLPlugin::writeAttribute( daeMetaAttribute* attr, daeElement* elemen
 		return;
 	}

-	if ( !attr->isArrayAttribute() && ( attr->getType()->getTypeEnum() == daeAtomicType::StringRefType || 
+	if ( !attr->isArrayAttribute() && ( attr->getType()->getTypeEnum() == daeAtomicType::StringRefType ||
 		 attr->getType()->getTypeEnum() == daeAtomicType::TokenType ) &&
 		 *(char**)attr->getWritableMemory( element ) != NULL )
 	{
@@ -865,7 +867,7 @@ void daeLIBXMLPlugin::writeAttribute( daeMetaAttribute* attr, daeElement* elemen
 				//early out if !value && !required && !set
 				return;
 			}
-			
+
 			//is set
 			//check for default suppression
 			if ( attr->getDefault() != NULL )
@@ -888,7 +890,7 @@ void daeLIBXMLPlugin::writeAttribute( daeMetaAttribute* attr, daeElement* elemen
 			xmlTextWriterStartAttribute( writer, (xmlChar*)(daeString)attr->getName() );
 		}
 	}
-	if (valCount>0) 
+	if (valCount>0)
 	{
 		//do val 0 first then space and the rest of the vals.
 		char* elemMem = attr->get( element, 0 );
@@ -899,7 +901,7 @@ void daeLIBXMLPlugin::writeAttribute( daeMetaAttribute* attr, daeElement* elemen
 		}

 		*buf = ' ';
-		for( size_t i = 1; i < valCount; i++ ) 
+		for( size_t i = 1; i < valCount; i++ )
 		{
 			elemMem = attr->get( element, (int)i );
 			attr->getType()->memoryToString( elemMem, buf+1, bufSz );
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -76,8 +76,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
 	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
 	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
 	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
 	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
@@ -114,9 +114,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	{
 		c.m_appliedImpulse = sum;
 	}
-	if (body1.m_invMass)
 		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
-	if (body2.m_invMass)
 		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
 }

@@ -138,8 +136,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
 	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
 	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
 	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
 	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
@@ -169,10 +167,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	{
 		c.m_appliedImpulse = sum;
 	}
-	if (body1.m_invMass)
-		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
-	if (body2.m_invMass)
-		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+	body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+	body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
 }


@@ -224,14 +220,14 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod

 	if (rb)
 	{
-		solverBody->m_invMass = rb->getInvMass();
+		solverBody->m_invMass = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 	} else
 	{
-		solverBody->m_invMass = 0.f;
+		solverBody->m_invMass.setValue(0,0,0);
 		solverBody->m_originalBody = 0;
-		solverBody->m_angularFactor = 1.f;
+		solverBody->m_angularFactor.setValue(1,1,1);
 	}
 }

@@ -494,13 +490,13 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m


 				///warm starting (or zero if disabled)
-				if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+				if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 				{
 					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 					if (rb0)
-						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
 					if (rb1)
-						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
 				} else
 				{
 					solverConstraint.m_appliedImpulse = 0.f;
@@ -587,9 +583,9 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 							{
 								frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 								if (rb0)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
 								if (rb1)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
 							} else
 							{
 								frictionConstraint1.m_appliedImpulse = 0.f;
--- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -110,8 +110,8 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 	btVector3		m_deltaLinearVelocity;
 	btVector3		m_deltaAngularVelocity;
-	btScalar		m_angularFactor;
-	btScalar		m_invMass;
+	btVector3		m_angularFactor;
+	btVector3		m_invMass;
 	btScalar		m_friction;
 	btRigidBody*	m_originalBody;
 	btVector3		m_pushVelocity;
@@ -162,7 +162,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody

 	void	writebackVelocity(btScalar timeStep=0)
 	{
-		if (m_invMass)
+		if (m_originalBody)
 		{
 			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
 			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
--- a/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -44,7 +44,8 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&

 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	m_angularFactor = btScalar(1.);
+	m_angularFactor.setValue(1,1,1);
+	m_linearFactor.setValue(1,1,1);
 	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -45,7 +45,8 @@ class btRigidBody  : public btCollisionObject
 	btVector3		m_linearVelocity;
 	btVector3		m_angularVelocity;
 	btScalar		m_inverseMass;
-	btScalar		m_angularFactor;
+	btVector3		m_angularFactor;
+	btVector3		m_linearFactor;

 	btVector3		m_gravity;	
 	btVector3		m_gravity_acceleration;
@@ -219,6 +220,14 @@ public:
 	
 	void			setMassProps(btScalar mass, const btVector3& inertia);
 	
+	const btVector3& getLinearFactor() const
+	{
+		return m_linearFactor;
+	}
+	void setLinearFactor(const btVector3& linearFactor)
+	{
+		m_linearFactor = linearFactor;
+	}
 	btScalar		getInvMass() const { return m_inverseMass; }
 	const btMatrix3x3& getInvInertiaTensorWorld() const { 
 		return m_invInertiaTensorWorld; 
@@ -230,7 +239,7 @@ public:

 	void			applyCentralForce(const btVector3& force)
 	{
-		m_totalForce += force;
+		m_totalForce += force*m_linearFactor;
 	}

 	const btVector3& getTotalForce()
@@ -261,23 +270,23 @@ public:

 	void	applyTorque(const btVector3& torque)
 	{
-		m_totalTorque += torque;
+		m_totalTorque += torque*m_angularFactor;
 	}
 	
 	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
 	{
 		applyCentralForce(force);
-		applyTorque(rel_pos.cross(force)*m_angularFactor);
+		applyTorque(rel_pos.cross(force*m_linearFactor));
 	}
 	
 	void applyCentralImpulse(const btVector3& impulse)
 	{
-		m_linearVelocity += impulse * m_inverseMass;
+		m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
 	}
 	
  	void applyTorqueImpulse(const btVector3& torque)
 	{
-			m_angularVelocity += m_invInertiaTensorWorld * torque;
+			m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
 	}
 	
 	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
@@ -287,7 +296,7 @@ public:
 			applyCentralImpulse(impulse);
 			if (m_angularFactor)
 			{
-				applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
+				applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
 			}
 		}
 	}
@@ -297,10 +306,10 @@ public:
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
-			m_linearVelocity += linearComponent*impulseMagnitude;
+			m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude;
 			if (m_angularFactor)
 			{
-				m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
+				m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude;
 			}
 		}
 	}
@@ -450,11 +459,16 @@ public:
 	int	m_contactSolverType;
 	int	m_frictionSolverType;

-	void	setAngularFactor(btScalar angFac)
+	void	setAngularFactor(const btVector3& angFac)
 	{
 		m_angularFactor = angFac;
 	}
-	btScalar	getAngularFactor() const
+
+	void	setAngularFactor(btScalar angFac)
+	{
+		m_angularFactor.setValue(angFac,angFac,angFac);
+	}
+	const btVector3&	getAngularFactor() const
 	{
 		return m_angularFactor;
 	}
--- a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
+++ b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
@@ -224,14 +224,14 @@ static void setupSpuBody (btCollisionObject* collisionObject, btSolverBody* solv

 	if (rb)
 	{
-		solverBody->m_invMass = rb->getInvMass();
+		solverBody->m_invMass.setValue(rb->getInvMass(),rb->getInvMass(),rb->getInvMass());
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 	} else
 	{
-		solverBody->m_invMass = 0.f;
+		solverBody->m_invMass.setValue(0,0,0);
 		solverBody->m_originalBody = 0;
-		solverBody->m_angularFactor = 1.f;
+		solverBody->m_angularFactor.setValue(1,1,1);
 	}
 	
 }
@@ -439,8 +439,8 @@ static void SpuResolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBod
 	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
 	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
 	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
-	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128);
+	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,body2.m_invMass.mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
 	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
 	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));