From 3b9b8036832ac877c013b292165d61bc04a72782 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 6 Nov 2015 17:11:15 -0800 Subject: [PATCH] b3CreateBoxCommandSetColorRGBA: allow to specify color when creating bodies through shared memory API Parse and use colors from URDF file (single rgba color per link, not per visual) Rename btMultiBody 'stepVelocities' to 'computeAccelerationsArticulatedBodyAlgorithmMultiDof' btHashMap, add const Value* operator[] remove a few more obsolete btMultiBody methods (on the non-multi-dof path) fix spelling typo in fillConstraintJacobianMultiDof (fil -> fill) Add mention to Jakub Stepien for his work on btMultiBody --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 27 +- .../ImportURDFDemo/BulletUrdfImporter.h | 2 + .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 6 +- .../ImportURDFDemo/URDFImporterInterface.h | 2 + .../Importers/ImportURDFDemo/UrdfParser.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 12 + examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsClientExample.cpp | 2 + examples/SharedMemory/PhysicsServer.cpp | 12 +- examples/SharedMemory/SharedMemoryCommands.h | 2 + .../Featherstone/btMultiBody.cpp | 470 +----------------- src/BulletDynamics/Featherstone/btMultiBody.h | 63 +-- .../btMultiBodyConstraintSolver.cpp | 57 +-- .../btMultiBodyConstraintSolver.h | 2 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 18 +- src/LinearMath/btHashMap.h | 4 + 16 files changed, 96 insertions(+), 588 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 05dc23d90..549c36b18 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -37,7 +37,7 @@ struct BulletURDFInternalData UrdfParser m_urdfParser; struct GUIHelperInterface* m_guiHelper; char m_pathPrefix[1024]; - + btHashMap m_linkColors; }; void BulletURDFImporter::printTree() @@ -96,6 +96,8 @@ struct BulletErrorLogger : public ErrorLogger bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { + m_data->m_linkColors.clear(); + //int argc=0; char relativeFileName[1024]; @@ -172,6 +174,17 @@ void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray } } +bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const +{ + btVector4* rgbaPtr = m_data->m_linkColors[linkIndex]; + if (rgbaPtr) + { + colorRGBA = *rgbaPtr; + return true; + } + return false; +} + std::string BulletURDFImporter::getLinkName(int linkIndex) const { UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); @@ -852,8 +865,18 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP for (int v = 0; v < link->m_visualArray.size();v++) { const UrdfVisual& vis = link->m_visualArray[v]; - btTransform childTrans = vis.m_linkLocalFrame; + btTransform childTrans = vis.m_linkLocalFrame; + btHashString matName(vis.m_materialName.c_str()); + UrdfMaterial *const * matPtr = model.m_materials[matName]; + if (matPtr) + { + UrdfMaterial *const mat = *matPtr; + //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); + m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); + } convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices); + + } } #endif diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 7f712e1cd..2b9e447bc 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -27,6 +27,8 @@ public: virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; virtual std::string getLinkName(int linkIndex) const; + + virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; virtual std::string getJointName(int linkIndex) const; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index ff93b7f5a..6d87d2ea3 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -24,7 +24,7 @@ static btVector4 colors[4] = }; -static btVector3 selectColor2() +static btVector4 selectColor2() { static int curColor = 0; @@ -371,8 +371,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); - btVector3 color = selectColor2();//(0.0,0.0,0.5); - + btVector4 color = selectColor2();//(0.0,0.0,0.5); + u2b.getLinkColor(urdfLinkIndex,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); btScalar friction = 0.5f; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 622486aa4..8fce7b3e6 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -24,6 +24,8 @@ public: ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) virtual std::string getLinkName(int linkIndex) const =0; + /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise + virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;} virtual std::string getJointName(int linkIndex) const = 0; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index cb3ed0816..a40d864fa 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -346,6 +346,8 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg { if (parseMaterial(visual.m_localMaterial, mat,logger)) { + UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); + m_model.m_materials.insert(matPtr->m_name.c_str(),matPtr); visual.m_hasLocalMaterial = true; } } @@ -786,7 +788,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial) + if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = m_model.m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 75578acb2..4784c87de 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -284,6 +284,18 @@ int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandH return 0; } +int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_COLOR; + command->m_createBoxShapeArguments.m_colorRGBA[0] = red; + command->m_createBoxShapeArguments.m_colorRGBA[1] = green; + command->m_createBoxShapeArguments.m_colorRGBA[2] = blue; + command->m_createBoxShapeArguments.m_colorRGBA[3] = alpha; + return 0; +} int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c1590f562..e4b4f2f71 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -94,6 +94,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ); int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass); int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType); +int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha); ///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles. diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 21ab8be51..fadd02e65 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -211,6 +211,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3); + b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -220,6 +221,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0); b3CreateBoxCommandSetMass(commandHandle,1); b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y); + b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1); double radius = 0.2; double halfHeight = 0.5; b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius); diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 4d2deb31e..d4f598834 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1684,7 +1684,17 @@ void PhysicsServerSharedMemory::processClientCommands() bool isDynamic = (mass>0); btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); rb->setRollingFriction(0.2); - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + btVector4 colorRGBA(1,0,0,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) + { + colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; + colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; + colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; + colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; + } + m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); + m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RIGID_BODY_CREATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); int bodyUniqueId = m_data->allocHandle(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 11a99917e..1e54b011b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -205,6 +205,7 @@ enum EnumBoxShapeFlags BOX_SHAPE_HAS_HALF_EXTENTS=4, BOX_SHAPE_HAS_MASS=8, BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE=16, + BOX_SHAPE_HAS_COLOR=32, }; ///This command will be replaced to allow arbitrary collision shape types struct CreateBoxShapeArgs @@ -218,6 +219,7 @@ struct CreateBoxShapeArgs double m_initialPosition[3]; double m_initialOrientation[4]; + double m_colorRGBA[4]; }; struct SharedMemoryCommand diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 96ef4e87e..be93a818d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -6,7 +6,8 @@ * * COPYRIGHT: * Copyright (C) Stephen Thompson, , 2011-2013 - * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -638,7 +639,7 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //r #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) // -void btMultiBody::stepVelocitiesMultiDof(btScalar dt, +void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m, @@ -654,6 +655,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), // num_links joint acceleration values. + // We added support for multi degree of freedom (multi dof) joints. + // In addition we also can compute the joint reaction forces. This is performed in a second pass, + // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver) + m_internalNeedsJointFeedback = false; int num_links = getNumLinks(); @@ -1181,320 +1186,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, } -void btMultiBody::stepVelocities(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) -{ - // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) - // and the base linear & angular accelerations. - - // We apply damping forces in this routine as well as any external forces specified by the - // caller (via addBaseForce etc). - - // output should point to an array of 6 + num_links reals. - // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), - // num_links joint acceleration values. - - int num_links = getNumLinks(); - - const btScalar DAMPING_K1_LINEAR = m_linearDamping; - const btScalar DAMPING_K2_LINEAR = m_linearDamping; - - const btScalar DAMPING_K1_ANGULAR = m_angularDamping; - const btScalar DAMPING_K2_ANGULAR= m_angularDamping; - - btVector3 base_vel = getBaseVel(); - btVector3 base_omega = getBaseOmega(); - - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame - - scratch_r.resize(2*num_links + 6); - scratch_v.resize(8*num_links + 6); - scratch_m.resize(4*num_links + 4); - - btScalar * r_ptr = &scratch_r[0]; - btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results - btVector3 * v_ptr = &scratch_v[0]; - - // vhat_i (top = angular, bottom = linear part) - btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // zhat_i^A - btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // chat_i (note NOT defined for the base) - btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; - btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; - - // top left, top right and bottom left blocks of Ihat_i^A. - // bottom right block = transpose of top left block and is not stored. - // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. - btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; - btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; - btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; - - // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - // hhat_i, ahat_i - // hhat is NOT stored for the base (but ahat is) - btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0; - btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0; - btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; - - // Y_i, D_i - btScalar * Y1 = r_ptr; r_ptr += num_links; - btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0; - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; - - - // Start of the algorithm proper. - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - rot_from_parent[0] = btMatrix3x3(m_baseQuat); - - vel_top_angular[0] = rot_from_parent[0] * base_omega; - vel_bottom_linear[0] = rot_from_parent[0] * base_vel; - - if (m_fixedBase) { - f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0); - } else { - f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce - - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); - - f_zero_acc_bottom_linear[0] = - - (rot_from_parent[0] * m_baseTorque); - - if (m_useGyroTerm) - f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] ); - - f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); - - } - - - - inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - - - inertia_top_right[0].setValue(m_baseMass, 0, 0, - 0, m_baseMass, 0, - 0, 0, m_baseMass); - inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0, - 0, m_baseInertia[1], 0, - 0, 0, m_baseInertia[2]); - - rot_from_world[0] = rot_from_parent[0]; - - for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); - - - rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, - vel_top_angular[parent+1], vel_bottom_linear[parent+1], - vel_top_angular[i+1], vel_bottom_linear[i+1]); - - // we can now calculate chat_i - // remember vhat_i is really vhat_p(i) (but in current frame) at this point - coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector)) - + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i); - if (m_links[i].m_jointType == btMultibodyLink::eRevolute) { - coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i); - coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0)); - } else { - coriolis_top_angular[i] = btVector3(0,0,0); - } - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); - vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); - - // calculate zhat_i^A - f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce)); - f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; - - f_zero_acc_bottom_linear[i+1] = - - (rot_from_world[i+1] * m_links[i].m_appliedTorque); - if (m_useGyroTerm) - { - f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] ); - } - - f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); - - // calculate Ihat_i^A - inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0, - 0, m_links[i].m_mass, 0, - 0, 0, m_links[i].m_mass); - inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0, - 0, m_links[i].m_inertiaLocal[1], 0, - 0, 0, m_links[i].m_inertiaLocal[2]); - } - - - // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) { - - h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0); - h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0); - btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]); - D[i] = val; - //Y1 = joint torque - zero acceleration force - coriolis force - Y1[i] = m_links[i].m_jointTorque[0] - - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1]) - - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); - - const int parent = m_links[i].m_parent; - - btAssert(D[i]!=0.f); - // Ip += pXi * (Ii - hi hi' / Di) * iXp - const btScalar one_over_di = 1.0f / D[i]; - - - - - const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]); - const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]); - const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]); - - - btMatrix3x3 r_cross; - r_cross.setValue( - 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1], - m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0], - -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0); - - inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; - inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; - inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() * - (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1]; - - - // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di) - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar Y_over_D = Y1[i] * one_over_di; - in_top = f_zero_acc_top_angular[i+1] - + inertia_top_left[i+1] * coriolis_top_angular[i] - + inertia_top_right[i+1] * coriolis_bottom_linear[i] - + Y_over_D * h_top[i]; - in_bottom = f_zero_acc_bottom_linear[i+1] - + inertia_bottom_left[i+1] * coriolis_top_angular[i] - + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i] - + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, - in_top, in_bottom, out_top, out_bottom); - f_zero_acc_top_angular[parent+1] += out_top; - f_zero_acc_bottom_linear[parent+1] += out_bottom; - } - - - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (m_fixedBase) - { - accel_top[0] = accel_bottom[0] = btVector3(0,0,0); - } - else - { - if (num_links > 0) - { - //Matrix Imatrix; - //Imatrix.block<3,3>(0,0) = inertia_top_left[0]; - //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0]; - //Imatrix.block<3,3>(0,3) = inertia_top_right[0]; - //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose(); - //cached_imatrix_lu.reset(new Eigen::LU >(Imatrix)); // TODO: Avoid memory allocation here? - - m_cachedInertiaTopLeft = inertia_top_left[0]; - m_cachedInertiaTopRight = inertia_top_right[0]; - m_cachedInertiaLowerLeft = inertia_bottom_left[0]; - m_cachedInertiaLowerRight= inertia_top_left[0].transpose(); - - } - btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]); - btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]); - float result[6]; - - solveImatrix(rhs_top, rhs_bot, result); -// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); - for (int i = 0; i < 3; ++i) { - accel_top[0][i] = -result[i]; - accel_bottom[0][i] = -result[i+3]; - } - - } - - // now do the loop over the m_links - for (int i = 0; i < num_links; ++i) - { - //acceleration of the child link = acceleration of the parent transformed into child frame + - // + coriolis acceleration - // + joint acceleration - const int parent = m_links[i].m_parent; - SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, - accel_top[parent+1], accel_bottom[parent+1], - accel_top[i+1], accel_bottom[i+1]); - - - joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0); - accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0); - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - - ///////////////// - //printf("q = ["); - //printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); - //for(int link = 0; link < getNumLinks(); ++link) - // printf("%.2f ", m_links[link].m_jointPos[0]); - //printf("]\n"); - //// - //printf("qd = ["); - //for(int dof = 0; dof < getNumLinks() + 6; ++dof) - // printf("%.2f ", m_realBuf[dof]); - //printf("]\n"); - //// - //printf("qdd = ["); - //for(int dof = 0; dof < getNumLinks() + 6; ++dof) - // printf("%.2f ", output[dof]); - //printf("]\n"); - ///////////////// - - // Final step: add the accelerations (times dt) to the velocities. - //todo: do we already need to update the velocities, or can we move this into the constraint solver? - //the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach - applyDeltaVee(output, dt); - - -} void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const { @@ -1762,151 +1453,6 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar } -void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const -{ - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame - int num_links = getNumLinks(); - scratch_r.resize(num_links); - scratch_v.resize(4*num_links + 4); - - btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0]; - btVector3 * v_ptr = &scratch_v[0]; - - // zhat_i^A (scratch space) - btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // rot_from_parent (cached from calcAccelerations) - const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; - - // hhat (cached), accel (scratch) - const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0; - const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0; - btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; - - // Y_i (scratch), D_i (cached) - btScalar * Y = r_ptr; r_ptr += num_links; - const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0; - - btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - btVector3 input_force(force[3],force[4],force[5]); - btVector3 input_torque(force[0],force[1],force[2]); - - // Fill in zero_acc - // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) - { - zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); - } else - { - zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force); - zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); - } - for (int i = 0; i < num_links; ++i) - { - zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0); - } - - // 'Downward' loop. - for (int i = num_links - 1; i >= 0; --i) - { -// btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); - Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); - Y[i] += force[6 + i]; // add joint torque - - const int parent = m_links[i].m_parent; - - // Zp += pXi * (Zi + hi*Yi/Di) - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar Y_over_D = Y[i] / D[i]; - in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, - in_top, in_bottom, out_top, out_bottom); - zero_acc_top_angular[parent+1] += out_top; - zero_acc_bottom_linear[parent+1] += out_bottom; - } - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; - - // Second 'upward' loop - if (m_fixedBase) - { - accel_top[0] = accel_bottom[0] = btVector3(0,0,0); - } else - { - btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); - btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); - - float result[6]; - solveImatrix(rhs_top,rhs_bot, result); - // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); - - for (int i = 0; i < 3; ++i) { - accel_top[0][i] = -result[i]; - accel_bottom[0][i] = -result[i+3]; - } - - } - - // now do the loop over the m_links - for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector, - accel_top[parent+1], accel_bottom[parent+1], - accel_top[i+1], accel_bottom[i+1]); - btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])); - joint_accel[i] = Y_minus_hT_a / D[i]; - accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0); - accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0); - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out; - omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - btVector3 vdot_out; - vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; - - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - - - ///////////////// - /* - int ndof = getNumLinks() + 6; - printf("test force(impulse) (%d) = [\n",ndof); - - for (int i=0;i, 2011-2013 - * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -30,6 +31,8 @@ #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" + +///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms #ifdef BT_USE_DOUBLE_PRECISION #define btMultiBodyData btMultiBodyDoubleData #define btMultiBodyDataName "btMultiBodyDoubleData" @@ -334,72 +337,26 @@ void addJointTorque(int i, btScalar Q); // improvement, at least on Windows (where dynamic memory // allocation appears to be fairly slow). // - void stepVelocities(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m); - - void stepVelocitiesMultiDof(btScalar dt, + + void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m, bool isConstraintPass=false ); - // calcAccelerationDeltas + // calcAccelerationDeltasMultiDof // input: force vector (in same format as jacobian, i.e.: // 3 torque values, 3 force values, num_links joint torque values) // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values // (existing contents of output array are replaced) // stepVelocities must have been called first. - void calcAccelerationDeltas(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v) const; void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const; - // apply a delta-vee directly. used in sequential impulses code. - void applyDeltaVee(const btScalar * delta_vee) - { - - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - m_realBuf[i] += delta_vee[i]; - } - - } - void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) - { - btScalar sum = 0; - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; - } - btScalar l = btSqrt(sum); - /* - static btScalar maxl = -1e30f; - if (l>maxl) - { - maxl=l; - // printf("maxl=%f\n",maxl); - } - */ - if (l>m_maxAppliedImpulse) - { -// printf("exceeds 100: l=%f\n",maxl); - multiplier *= m_maxAppliedImpulse/l; - } - - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; - m_realBuf[i] += delta_vee[i] * multiplier; - btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); - } - } - + void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) { for (int dof = 0; dof < 6 + getNumDofs(); ++dof) @@ -462,11 +419,11 @@ void addJointTorque(int i, btScalar Q); btScalar *jac, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } + btAlignedObjectArray &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } //a more general version of fillContactJacobianMultiDof which does not assume.. //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only - void filConstraintJacobianMultiDof(int link, + void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 9f2e84aba..8a034b38a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -34,8 +34,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl for (int j=0;jsetPosUpdated(false); @@ -192,60 +191,6 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult } -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = c.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } - - if (c.m_multiBodyB) - { - ndofB = c.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } - - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } -} void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 0ec06bf55..321ee4231 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -44,7 +44,7 @@ protected: int m_tmpNumMultiBodyConstraints; void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c); + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 13676cc7e..a9c0b33b3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -508,7 +508,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { if(!bod->isUsingRK4Integration()) { - bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); } else { @@ -596,7 +596,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btScalar h = solverInfo.m_timeStep; #define output &scratch_r[bod->getNumDofs()] //calc qdd0 from: q0 & qd0 - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); @@ -606,7 +606,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd1 from: q1 & qd1 pCopyToVelocityVector(bod, scratch_qd1); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); @@ -616,7 +616,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd2 from: q2 & qd2 pCopyToVelocityVector(bod, scratch_qd2); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); @@ -626,7 +626,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); pCopy(output, scratch_qdd3, 0, numDofs); // @@ -661,7 +661,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { for(int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m); } } @@ -710,7 +710,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if(!bod->isUsingRK4Integration()) { bool isConstraintPass = true; - bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); } } } @@ -718,8 +718,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; + { + btMultiBody* bod = m_multiBodies[i]; bod->processDeltaVeeMultiDof2(); } diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index ce07db3ac..af9727b7a 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -399,6 +399,10 @@ protected: return find(key); } + const Value* operator[](const Key& key) const { + return find(key); + } + const Value* find(const Key& key) const { int index = findIndex(key);