From eac8b3278235da3132347f6c340e53a69a8c7298 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 31 Oct 2014 14:14:45 -0700 Subject: [PATCH] add very basic multibody vehicle tweak finite element experiment with parameter --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 11 +- Demos3/AllBullet2Demos/premake4.lua | 4 +- .../FiniteElementMethod/FiniteElementDemo.cpp | 79 +++++- Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 253 +----------------- .../ConstraintDemo/ConstraintPhysicsSetup.cpp | 204 ++++++++------ .../MultiBodyDemo/MultiBodyVehicle.cpp | 236 ++++++++++++++++ .../bullet2/MultiBodyDemo/MultiBodyVehicle.h | 24 ++ .../CommonParameterInterface.h | 9 +- .../GwenParameterInterface.cpp | 14 +- 9 files changed, 486 insertions(+), 348 deletions(-) create mode 100644 Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp create mode 100644 Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.h diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index e7c406b64..c0c4cb36e 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -22,6 +22,8 @@ #include "../../Demos/SerializeDemo/SerializeSetup.h" #include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h" +#include "../bullet2/MultiBodyDemo/MultiBodyVehicle.h" + #include "../bullet2/CollisionDetection/SupportFuncDemo.h" #include "../bullet2/BasicConcepts/CoordinateSystemDemo.h" #include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h" @@ -31,7 +33,11 @@ static BulletDemoInterface* TestJointTorqueCreateFunc(CommonGraphicsApp* app) CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup(); return new BasicDemo(app, physicsSetup); } - +static BulletDemoInterface* MultiBodyVehicleCreateFunc(CommonGraphicsApp* app) +{ + CommonPhysicsSetup* physicsSetup = new MultiBodyVehicleSetup(); + return new BasicDemo(app, physicsSetup); +} static BulletDemoInterface* LuaDemoCreateFunc(CommonGraphicsApp* app) { CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app); @@ -116,7 +122,7 @@ static BulletDemoEntry allDemos[]= { 1, "URDF", MyImportUrdfCreateFunc }, { 1, "STL", MyImportSTLCreateFunc}, { 1, "COLLADA", MyImportColladaCreateFunc}, - {0,"Finite Element Method", 0}, + {0,"Experiments", 0}, {1, "Finite Element Demo", FiniteElementDemo::CreateFunc}, /* {1,"ChainDemo",ChainDemo::MyCreateFunc}, // {0, "Stress tests", 0 }, @@ -135,6 +141,7 @@ static BulletDemoEntry allDemos[]= // {"MultiBody2",FeatherstoneDemo2::MyCreateFunc}, {1,"MultiDofDemo",MultiDofDemo::MyCreateFunc}, {1,"TestJointTorque",TestJointTorqueCreateFunc}, + {1,"MultiBodyVehicle", MultiBodyVehicleCreateFunc}, }; diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index 26125ce52..8030462e1 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -44,7 +44,9 @@ "../bullet2/LuaDemo/LuaPhysicsSetup.h", "../bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp", "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h", - -- "../DifferentialGearDemo/DifferentialGearSetup.cpp", + "../bullet2/MultiBodyDemo/MultiBodyVehicle.cpp", + "../bullet2/MultiBodyDemo/MultiBodyVehicle.h", +-- "../DifferentialGearDemo/DifferentialGearSetup.cpp", -- "../DifferentialGearDemo/DifferentialGearSetup.h", "../FiniteElementMethod/FiniteElementDemo.cpp", "../../Demos/BasicDemo/BasicDemoPhysicsSetup.cpp", diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp index fccdcb948..b445e20c9 100644 --- a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp +++ b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp @@ -26,6 +26,7 @@ subject to the following restrictions: #include #include #include "LinearMath/btAlignedObjectArray.h" +#include "Bullet3AppSupport/CommonParameterInterface.h" //typedef OpenTissue::math::BasicMathTypes math_types; typedef OpenTissue::math::BasicMathTypes math_types; @@ -34,7 +35,7 @@ typedef math_types::vector3_type vector3_type; typedef math_types::real_type real_type; - +static int fixedNodes = 1; struct FiniteElementDemoInternalData { @@ -48,8 +49,8 @@ struct FiniteElementDemoInternalData real_type m_gravity; - real_type m_young;// = 500000; - real_type m_poisson;// = 0.33; + btScalar m_young;// = 500000; + btScalar m_poisson;// = 0.33; real_type m_density;// = 1000; //--- infinite m_c_yield plasticity settings means that plasticity is turned off @@ -62,7 +63,8 @@ struct FiniteElementDemoInternalData { m_stiffness_warp_on= true; m_collideGroundPlane = true; - m_fixNodes = true; + m_fixNodes = fixedNodes==1; + fixedNodes=1-fixedNodes; m_gravity = 9.81; m_young = 500000;//47863;//100000; m_poisson = 0.33; @@ -100,13 +102,37 @@ void FiniteElementDemo::initPhysics() for (int n=0;nm_mesh1.m_nodes.size();n++) { - m_data->m_mesh1.m_nodes[n].m_coord(1)+=0.1f; + m_data->m_mesh1.m_nodes[n].m_coord(m_app->getUpAxis())+=.5f; m_data->m_mesh1.m_nodes[n].m_model_coord = m_data->m_mesh1.m_nodes[n].m_coord; } - OpenTissue::fem::init(m_data->m_mesh1,m_data->m_young,m_data->m_poisson,m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max); + OpenTissue::fem::init(m_data->m_mesh1,double(m_data->m_young),double(m_data->m_poisson),m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max); + + } + + { + + SliderParams slider("Young",&m_data->m_young); +// slider.m_showValues = false; + slider.m_minVal=50000; + slider.m_maxVal=1000000; + m_app->m_parameterInterface->registerSliderFloatParameter(slider); + } + + { + + SliderParams slider("Poisson",&m_data->m_poisson); + // slider.m_showValues = false; + slider.m_minVal=0.01; + slider.m_maxVal=0.49; + m_app->m_parameterInterface->registerSliderFloatParameter(slider); + } + + + + } void FiniteElementDemo::exitPhysics() { @@ -118,7 +144,10 @@ void FiniteElementDemo::stepSimulation(float deltaTime) m_y+=0.01f; m_z+=0.01f; double dt = 1./60.;//double (deltaTime); - //normal gravity + double poisson =m_data->m_poisson; + OpenTissue::fem::init(m_data->m_mesh1,double(m_data->m_young),poisson,m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max); + + for (int n=0;nm_mesh1.m_nodes.size();n++) { @@ -136,9 +165,39 @@ void FiniteElementDemo::stepSimulation(float deltaTime) m_data->m_mesh1.m_nodes[n].m_fixed = false; } } - vector3_type gravity = vector3_type(0.0, 0.0 , 0.0); - gravity(m_app->getUpAxis()) = -(m_data->m_mesh1.m_nodes[n].m_mass * m_data->m_gravity); - m_data->m_mesh1.m_nodes[n].m_f_external =gravity; + if (m_data->m_collideGroundPlane && m_data->m_mesh1.m_nodes[n].m_coord(m_app->getUpAxis())<0.f) + { + float depth = -m_data->m_mesh1.m_nodes[n].m_coord(m_app->getUpAxis()); + if (depth>0.1) + depth=0.1; + + m_data->m_mesh1.m_nodes[n].m_f_external(m_app->getUpAxis()) = depth*1000; + + if (m_data->m_mesh1.m_nodes[n].m_velocity(m_app->getUpAxis()) < 0.f) + { + m_data->m_mesh1.m_nodes[n].m_velocity(m_app->getUpAxis())=0.f; + } + + int frictionAxisA=0; + int frictionAxisB=2; + if (m_app->getUpAxis()==1) + { + frictionAxisA=0; + frictionAxisB=2; + } else + { + frictionAxisA=0; + frictionAxisB=1; + } + m_data->m_mesh1.m_nodes[n].m_velocity(frictionAxisA)=0.f; + m_data->m_mesh1.m_nodes[n].m_velocity(frictionAxisB)=0.f; + + } else + { + vector3_type gravity = vector3_type(0.0, 0.0 , 0.0); + gravity(m_app->getUpAxis()) = -(m_data->m_mesh1.m_nodes[n].m_mass * m_data->m_gravity); + m_data->m_mesh1.m_nodes[n].m_f_external =gravity; + } //m_data->m_mesh1.m_nodes[n].m_velocity.clear(); } diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index 85a7518e4..affa8c93e 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -348,256 +348,6 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c } return shape; } - -btMultiBody* URDF2BulletMultiBody(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints) -{ - - btScalar mass = 0.f; - btTransform localInertialTransform; localInertialTransform.setIdentity(); - btVector3 localInertiaDiagonal(0,0,0); - - { - - if ((*link).inertial) - { - mass = (*link).inertial->mass; - btMatrix3x3 inertiaMat; - inertiaMat.setIdentity(); - inertiaMat.setValue( - (*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz, - (*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz, - (*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz); - - btScalar threshold = 0.00001f; - int maxSteps=20; - btMatrix3x3 inertia2PrincipalAxis; - inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps); - localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]); - - btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z); - localInertialTransform.setOrigin(inertiaLocalCOM); - btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w); - btMatrix3x3 inertiaOrnMat(inertiaOrn); - - if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f - || localInertiaDiagonal[2] == 0.f)) - { - b3Warning("Error: inertia should not be zero if mass is positive\n"); - localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1)); - localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat); - } - else - { - localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis); - } - } - } - btTransform linkTransformInWorldSpace; - int parentIndex = -1; - - const Link* parentLink = (*link).getParent(); - if (parentLink) - { - parentIndex = parentLink->m_link_index; - btAssert(parentIndex>=0); - } - int linkIndex = mappings.m_linkMasses.size(); - - btTransform parent2joint; - - if ((*link).parent_joint) - { - const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position; - const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation; - parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); - parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); - linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - } else - { - linkTransformInWorldSpace = parentTransformInWorldSpace; - - btAssert(mb==0); - - bool multiDof = true; - bool canSleep = false; - bool isFixedBase = (mass==0);//todo: figure out when base is fixed - - mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); - - - } - - btAssert(mb); - - (*link).m_link_index = linkIndex; - - //compute this links center of mass transform, aligned with the principal axis of inertia - - - { - //btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame; - - mappings.m_linkMasses.push_back(mass); - mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal); - mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform); - - - - if ((*link).parent_joint) - { - btTransform offsetInA,offsetInB; - offsetInA.setIdentity(); - //offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint; - offsetInA = parent2joint; - offsetInB.setIdentity(); - //offsetInB = localInertialTransform.inverse(); - - const Joint* pj = (*link).parent_joint.get(); - //btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z); - btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z); - mappings.m_jointAxisArray.push_back(jointAxis); - mappings.m_jointOffsetInParent.push_back(offsetInA); - mappings.m_jointOffsetInChild.push_back(offsetInB); - mappings.m_jointTypeArray.push_back(pj->type); - - switch (pj->type) - { - case Joint::FIXED: - { - printf("Fixed joint\n"); - mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin()); - - break; - } - case Joint::CONTINUOUS: - case Joint::REVOLUTE: - { - printf("Revolute joint\n"); - mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true); - mb->finalizeMultiDof(); - //mb->setJointVel(linkIndex-1,1); - - break; - } - case Joint::PRISMATIC: - { - mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true); - printf("Prismatic joint\n"); - break; - } - default: - { - printf("Unknown joint\n"); - btAssert(0); - } - }; - - - - - } else - { - mappings.m_jointAxisArray.push_back(btVector3(0,0,0)); - btTransform ident; - ident.setIdentity(); - mappings.m_jointOffsetInParent.push_back(ident); - mappings.m_jointOffsetInChild.push_back(ident); - mappings.m_jointTypeArray.push_back(-1); - - - } - } - - //btCompoundShape* compoundShape = new btCompoundShape(); - btCollisionShape* shape = 0; - - for (int v=0;v<(int)link->collision_array.size();v++) - { - const Collision* visual = link->collision_array[v].get(); - - shape = convertVisualToCollisionShape(visual,pathPrefix); - - if (shape)//childShape) - { - gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape); - - btVector3 color = selectColor(); - /* - if (visual->material.get()) - { - color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); - } - */ - btVector3 localInertia(0,0,0); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - //btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia); - - - btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z); - btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w); - btTransform visual_frame; - visual_frame.setOrigin(visual_pos); - visual_frame.setRotation(visual_orn); - btTransform childTransform; - childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform - // compoundShape->addChildShape(childTransform,childShape); - } - } - - if (shape)//compoundShape->getNumChildShapes()>0) - { - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1); - col->setCollisionShape(shape); - - btTransform tr; - tr.setIdentity(); - tr = linkTransformInWorldSpace; - //if we don't set the initial pose of the btCollisionObject, the simulator will do this - //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider - - //tr.setOrigin(local_origin[0]); - //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - - bool isDynamic = true; - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); - - btVector3 color(0.0,0.0,0.5); - gfxBridge.createCollisionObjectGraphicsObject(col,color); - btScalar friction = 0.5f; - - col->setFriction(friction); - - if (parentIndex>=0) - { - mb->getLink(linkIndex-1).m_collider=col; - } else - { - mb->setBaseCollider(col); - } - } - - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints); - - } - else - { - std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl; - } - } - return mb; -} - void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix) { btCollisionShape* shape = 0; @@ -877,6 +627,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) } //the btMultiBody support is work-in-progress :-) +#if 0 if (0) { URDF2BulletMappings mappings; @@ -889,7 +640,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) //m_dynamicsWorld->integrateTransforms(0.f); } - +#endif// printf("numJoints/DOFS = %d\n", numJoints); diff --git a/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp index 9bf90c769..b1bed5888 100644 --- a/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp +++ b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp @@ -1,77 +1,127 @@ -#include "ConstraintPhysicsSetup.h" - -ConstraintPhysicsSetup::ConstraintPhysicsSetup() -{ -} -ConstraintPhysicsSetup::~ConstraintPhysicsSetup() -{ -} - -btScalar val; -btHingeAccumulatedAngleConstraint* spDoorHinge=0; -void ConstraintPhysicsSetup::stepSimulation(float deltaTime) -{ - val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD;// spDoorHinge->getHingeAngle()*SIMD_DEGS_PER_RAD; - if (m_dynamicsWorld) - { - m_dynamicsWorld->stepSimulation(deltaTime); - } -} - - -#include "Bullet3Common/b3Logging.h" - -void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) -{ - b3Printf(__FILE__); - - gfxBridge.setUpAxis(1); - - createEmptyDynamicsWorld(); - - gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); -int mode = btIDebugDraw::DBG_DrawWireframe - +btIDebugDraw::DBG_DrawConstraints - +btIDebugDraw::DBG_DrawConstraintLimits; - m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); - -val=1.f; - SliderParams slider("hinge angle",&val); -slider.m_minVal=-720; -slider.m_maxVal=720; - gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider); - - - { // create a door using hinge constraint attached to the world - btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); - m_collisionShapes.push_back(pDoorShape); - btTransform doorTrans; - doorTrans.setIdentity(); - doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f)); - btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape); - pDoorBody->setActivationState(DISABLE_DEACTIVATION); - const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside - btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis - - spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA ); - spDoorHinge->setParam(BT_CONSTRAINT_ERP,0.5); - btScalar erp = spDoorHinge->getParam(BT_CONSTRAINT_ERP); - -// spDoorHinge->setLimit( 0.0f, SIMD_PI_2 ); - // test problem values -// spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f); - -// spDoorHinge->setLimit( 1.f, -1.f); -// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI); -// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f); -// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits" - // spDoorHinge->setLimit( -SIMD_PI * 0.25f, SIMD_PI * 0.25f ); -// spDoorHinge->setLimit( 0.0f, 0.0f ); - m_dynamicsWorld->addConstraint(spDoorHinge); - spDoorHinge->setDbgDrawSize(btScalar(5.f)); - - //doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f)); - //btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape); - } - -} \ No newline at end of file +#include "ConstraintPhysicsSetup.h" + +ConstraintPhysicsSetup::ConstraintPhysicsSetup() +{ +} +ConstraintPhysicsSetup::~ConstraintPhysicsSetup() +{ +} + +static btScalar val; +static btScalar targetVel=0; +static btScalar maxImpulse=10000; +static btHingeAccumulatedAngleConstraint* spDoorHinge=0; +static btScalar actualHingeVelocity=0.f; + +static btVector3 btAxisA(0,1,0); + +void ConstraintPhysicsSetup::stepSimulation(float deltaTime) +{ + val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD; + if (m_dynamicsWorld) + { + spDoorHinge->enableAngularMotor(true,targetVel,maxImpulse); + + m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + + + btHingeConstraint* hinge = spDoorHinge; + + if (hinge) + { + + const btRigidBody& bodyA = hinge->getRigidBodyA(); + const btRigidBody& bodyB = hinge->getRigidBodyB(); + + + btTransform trA = bodyA.getWorldTransform(); + btVector3 angVelA = bodyA.getAngularVelocity(); + btVector3 angVelB = bodyB.getAngularVelocity(); + + { + btVector3 ax1 = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2); + btScalar vel = angVelA.dot(ax1); + vel -= angVelB.dot(ax1); + printf("hinge velocity (q) = %f\n", vel); + actualHingeVelocity=vel; + } + btVector3 ortho0,ortho1; + btPlaneSpace1(btAxisA,ortho0,ortho1); + { + + btScalar vel2 = angVelA.dot(ortho0); + vel2 -= angVelB.dot(ortho0); + printf("hinge orthogonal1 velocity (q) = %f\n", vel2); + } + { + + btScalar vel0 = angVelA.dot(ortho1); + vel0 -= angVelB.dot(ortho1); + printf("hinge orthogonal0 velocity (q) = %f\n", vel0); + } + } + } +} + + + +void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + gfxBridge.setUpAxis(1); + + createEmptyDynamicsWorld(); + + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); +int mode = btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawConstraintLimits; + m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); + + + { + SliderParams slider("target vel",&targetVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("max impulse",&maxImpulse); + slider.m_minVal=0; + slider.m_maxVal=1000; + gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("actual vel",&actualHingeVelocity); + slider.m_minVal=-4; + slider.m_maxVal=4; + gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider); + } + + val=1.f; + { + SliderParams slider("angle",&val); + slider.m_minVal=-720; + slider.m_maxVal=720; + gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider); + } + + { // create a door using hinge constraint attached to the world + btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); + m_collisionShapes.push_back(pDoorShape); + btTransform doorTrans; + doorTrans.setIdentity(); + doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f)); + btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape); + pDoorBody->setActivationState(DISABLE_DEACTIVATION); + const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside + + spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA ); + + m_dynamicsWorld->addConstraint(spDoorHinge); + + spDoorHinge->setDbgDrawSize(btScalar(5.f)); + } + +} diff --git a/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp b/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp new file mode 100644 index 000000000..e7823d281 --- /dev/null +++ b/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp @@ -0,0 +1,236 @@ +//test addJointTorque +#include "MultiBodyVehicle.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" + +btScalar gVehicleBaseMass = 100.f; +btScalar gVehicleWheelMass = 5.f; +float friction = 1.f; +btVector3 gVehicleBaseHalfExtents(1, 0.1, 2); +btVector3 gVehicleWheelHalfExtents(0.2, 0.2, 0.2); + +btVector3 gVehicleWheelOffset(0, 0, 0.5); +btVector3 wheelAttachmentPosInWorld[4] = { + btVector3(1, 0, 2.), + btVector3(-1, 0, 2.), + btVector3(1, 0, -2.), + btVector3(-1, 0, -2.) +}; + + +MultiBodyVehicleSetup::MultiBodyVehicleSetup() +{ +} + +MultiBodyVehicleSetup::~MultiBodyVehicleSetup() +{ + +} + + + +class btMultiBody* MultiBodyVehicleSetup::createMultiBodyVehicle() +{ + class btMultiBodyDynamicsWorld* world = m_dynamicsWorld; + int numWheels = 4; + + int totalLinks = numWheels;//number of body parts (links) (in)directly attached to the base, NOT including the base/root itself + + btCollisionShape* chassis = new btBoxShape(gVehicleBaseHalfExtents);//CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)); + m_collisionShapes.push_back(chassis); + btCollisionShape* wheel = new btCylinderShapeX(gVehicleWheelHalfExtents);//CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)); + m_collisionShapes.push_back(wheel); + + + btVector3 baseLocalInertia(0, 0, 0); + chassis->calculateLocalInertia(gVehicleBaseMass, baseLocalInertia); + + bool multiDof = false; + bool isFixedBase = false; + bool canSleep = false; + + btMultiBody * bod = new btMultiBody(totalLinks, gVehicleBaseMass, baseLocalInertia, isFixedBase, canSleep);// , multiDof); + bod->setHasSelfCollision(false); + + btQuaternion baseOrn(0, 0, 0, 1); + btVector3 basePos(0, 0, 0); + bod->setBasePos(basePos); + + bod->setWorldToBaseRot(baseOrn); + btVector3 vel(0, 0, 0); + bod->setBaseVel(vel); + + { + + + + + int linkNum = 0; + + + btVector3 wheelJointAxisWorld(1, 0, 0); + btQuaternion parent_to_child = baseOrn.inverse();//?? + for (int j = 0; j < numWheels; j++, linkNum++) + { + int parent_link_num = -1; + + float initial_joint_angle = 0.0; + + btVector3 localWheelInertia(0, 0, 0); + wheel->calculateLocalInertia(gVehicleWheelMass, localWheelInertia); + bool disableParentCollision = true; + btVector3 pivotToChildCOM(0, 0, 0.25); + btVector3 pivotToWheelCOM(0, 0, 0); + { + bod->setupRevolute(linkNum, gVehicleWheelMass, localWheelInertia, parent_link_num, parent_to_child, wheelJointAxisWorld, + wheelAttachmentPosInWorld[j], pivotToWheelCOM, disableParentCollision); + } + bod->setJointPos(linkNum, initial_joint_angle); + + if (j<2) +{ + btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod, linkNum, 1., 50); + world->addMultiBodyConstraint(con); +} + } + + + } + + //add a collider for the base + { + + btAlignedObjectArray world_to_local; + world_to_local.resize(totalLinks + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(totalLinks + 1); + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + { + + float pos[4] = { local_origin[0].x(), local_origin[0].y(), local_origin[0].z(), 1 }; + float quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() }; + + + if (1) + { + + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod, -1); + col->setCollisionShape(chassis); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);// 2, 1 + 2); + col->setFriction(friction); + bod->setBaseCollider(col); + } + } + + //initialize local coordinate frames, relative to parent + for (int i = 0; igetNumLinks(); i++) + { + const int parent = bod->getParent(i); + world_to_local[i + 1] = bod->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), bod->getRVector(i))); + } + + int linkIndex = 0; + + + + for (int j = 0; jsetCollisionShape(wheel); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);// 2, 1 + 2); + bod->getLink(linkIndex).m_collider = col; + } + } + + + world->addMultiBody(bod); +// world->setGravity(btVector3(0,0,0)); + + return bod; +} + + +void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + int upAxis = 1; + + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + int curColor = 0; + + + + gfxBridge.setUpAxis(upAxis); + + this->createEmptyDynamicsWorld(); + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + //btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); + + + createMultiBodyVehicle(); + + if (1) + { + btVector3 groundHalfExtents(20,20,20); + groundHalfExtents[upAxis]=1.f; + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); + + gfxBridge.createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 groundOrigin(0,0,0); + groundOrigin[upAxis]=-1.5; + start.setOrigin(groundOrigin); + btRigidBody* body = createRigidBody(0,start,box); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + gfxBridge.createRigidBodyGraphicsObject(body,color); + } + +} + +void MultiBodyVehicleSetup::stepSimulation(float deltaTime) +{ + m_dynamicsWorld->stepSimulation(deltaTime); +} + diff --git a/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.h b/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.h new file mode 100644 index 000000000..115160c68 --- /dev/null +++ b/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.h @@ -0,0 +1,24 @@ +#ifndef TEST_MULTIBODY_VEHICLE_SETUP_H +#define TEST_MULTIBODY_VEHICLE_SETUP_H + +#include "Bullet3AppSupport/CommonMultiBodySetup.h" + +struct MultiBodyVehicleSetup : public CommonMultiBodySetup +{ + btMultiBody* m_multiBody; + +public: + + MultiBodyVehicleSetup(); + virtual ~MultiBodyVehicleSetup(); + + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + + virtual void stepSimulation(float deltaTime); + + class btMultiBody* createMultiBodyVehicle(); + + +}; +#endif //TEST_MULTIBODY_VEHICLE_SETUP_H + diff --git a/btgui/Bullet3AppSupport/CommonParameterInterface.h b/btgui/Bullet3AppSupport/CommonParameterInterface.h index 55acc3e94..2f94d31b1 100644 --- a/btgui/Bullet3AppSupport/CommonParameterInterface.h +++ b/btgui/Bullet3AppSupport/CommonParameterInterface.h @@ -16,7 +16,8 @@ struct SliderParams btScalar* m_paramValuePointer; void* m_userPointer; bool m_clampToNotches; - + bool m_showValues; + SliderParams(const char* name, btScalar* targetValuePointer) :m_name(name), m_minVal(-100), @@ -24,7 +25,8 @@ struct SliderParams m_callback(0), m_paramValuePointer(targetValuePointer), m_userPointer(0), - m_clampToNotches(false) + m_clampToNotches(false), + m_showValues(true) { } @@ -33,7 +35,8 @@ struct SliderParams struct CommonParameterInterface { - virtual void registerSliderFloatParameter(SliderParams& params)=0; + + virtual void registerSliderFloatParameter(SliderParams& params)=0; virtual void syncParameters()=0; virtual void removeAllParameters()=0; virtual void setSliderValue(int sliderIndex, double sliderValue)=0; diff --git a/btgui/Bullet3AppSupport/GwenParameterInterface.cpp b/btgui/Bullet3AppSupport/GwenParameterInterface.cpp index c1c1133f1..8c38528b3 100644 --- a/btgui/Bullet3AppSupport/GwenParameterInterface.cpp +++ b/btgui/Bullet3AppSupport/GwenParameterInterface.cpp @@ -10,11 +10,13 @@ struct MySliderEventHandler : public Gwen::Event::Handler Gwen::Controls::Slider* m_pSlider; char m_variableName[1024]; T* m_targetValue; + bool m_showValue; MySliderEventHandler(const char* varName, Gwen::Controls::TextBox* label, Gwen::Controls::Slider* pSlider,T* target) :m_label(label), m_pSlider(pSlider), - m_targetValue(target) + m_targetValue(target), + m_showValue(true) { memcpy(m_variableName,varName,strlen(varName)+1); } @@ -45,9 +47,12 @@ struct MySliderEventHandler : public Gwen::Event::Handler m_pSlider->SetValue(v,true); (*m_targetValue) = v; float val = float(v);//todo: specialize on template type - char txt[1024]; - sprintf(txt,"%s : %.3f", m_variableName,val); - m_label->SetText(txt); + if (m_showValue) + { + char txt[1024]; + sprintf(txt,"%s : %.3f", m_variableName,val); + m_label->SetText(txt); + } } }; @@ -120,6 +125,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) char labelName[1024]; sprintf(labelName,"%s",params.m_name);//axisNames[0]); MySliderEventHandler* handler = new MySliderEventHandler(labelName,label,pSlider,params.m_paramValuePointer); + handler->m_showValue = params.m_showValues; m_paramInternalData->m_sliderEventHandlers.push_back(handler); pSlider->onValueChanged.Add( handler, &MySliderEventHandler::SliderMoved );