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);