diff --git a/Demos/ForkLiftDemo/CMakeLists.txt b/Demos/ForkLiftDemo/CMakeLists.txt index 6a930af77..fd3ef5a70 100644 --- 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 diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 2528ed391..6defcb78c 100644 --- 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(); diff --git a/Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp b/Extras/COLLADA_DOM/src/modules/LIBXMLPlugin/daeLIBXMLPlugin.cpp index 93cfe10f5..5c7e6d807 100644 --- 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 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;icreateFromChecked(intItems[i].element); - + for (i=0;ifromCOLLADAChecked(); @@ -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;igetValueAttribute(); 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 ); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 6d32ee70e..685a812d4 100644 --- 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; diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 98c9876ae..6b7289591 100644 --- 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); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index a2b8a7dad..a4d8e1d77 100644 --- 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)); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index ec570cab8..da1fcb786 100644 --- 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; } diff --git a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp index d215100c9..3a242244f 100644 --- 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));