bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) { const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape(); btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); btScalar boxMargin = boxShape->getMargin(); penetrationDepth = 1.0f; // convert the sphere position to the box's local space btTransform const &m44T = boxObjWrap->getWorldTransform(); btVector3 sphereRelPos = m44T.invXform(sphereCenter); // Determine the closest point to the sphere center in the box btVector3 closestPoint = sphereRelPos; closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) ); closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) ); closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) ); closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) ); closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) ); closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) ); btScalar intersectionDist = fRadius + boxMargin; btScalar contactDist = intersectionDist + maxContactDistance; normal = sphereRelPos - closestPoint; //if there is no penetration, we are done btScalar dist2 = normal.length2(); if (dist2 > contactDist * contactDist) { return false; } btScalar distance; //special case if the sphere center is inside the box if (dist2 == 0.0f) { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); } else //compute the penetration details { distance = normal.length(); normal /= distance; } pointOnBox = closestPoint + normal * boxMargin; // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; // transform back in world space btVector3 tmp = m44T(pointOnBox); pointOnBox = tmp; // tmp = m44T(v3PointOnSphere); // v3PointOnSphere = tmp; tmp = m44T.getBasis() * normal; normal = tmp; return true; }
btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) { m_simplexSolver = new btVoronoiSimplexSolver(); if (constructionInfo.m_useEpaPenetrationAlgorithm) { m_pdSolver = new btGjkEpaPenetrationDepthSolver; }else { m_pdSolver = new btMinkowskiPenetrationDepthSolver; } //default CreationFunctions, filling the m_doubleDispatch table m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc; m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc; m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc; m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc; #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc; m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc; m_boxSphereCF->m_swapped = true; #endif //USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereTriangleCF = new btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF = new btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF->m_swapped = true; m_boxBoxCF = new btBoxBoxCollisionAlgorithm::CreateFunc; //convex versus plane m_convexPlaneCF = new btConvexPlaneCollisionAlgorithm::CreateFunc; m_planeConvexCF = new btConvexPlaneCollisionAlgorithm::CreateFunc; m_planeConvexCF->m_swapped = true; ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); int sl = sizeof(btConvexSeparatingDistanceUtil); sl = sizeof(btGjkPairDetector); int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); }
btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) :btDefaultCollisionConfiguration(constructionInfo) { void* mem; mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; m_swappedSoftRigidConvexCreateFunc->m_swapped=true; #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; #endif //replace pool by a new one, with potential larger size if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) { int curElemSize = m_collisionAlgorithmPool->getElementSize(); ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); if (collisionAlgorithmMaxElementSize > curElemSize) { m_collisionAlgorithmPool->~btPoolAllocator(); btAlignedFree(m_collisionAlgorithmPool); void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } } }
void Vehicle::SetBraking(float braking) { braking = btMin(btMax(0.f, braking), 1.f);//clamp brakingForce = maxBrakingForce * braking; vehicle->setBrake(brakingForce, W_RL); vehicle->setBrake(brakingForce, W_RR); }
static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) { if(node->isinternal()) { getmaxdepth(node->childs[0],depth+1,maxdepth); getmaxdepth(node->childs[1],depth+1,maxdepth); } else maxdepth=btMax(maxdepth,depth); }
void Vehicle::SetThrottle(float throttle) { throttle = btMin(btMax(-1.f, throttle), 1.f);//clamp engineForce = maxEngineForce * throttle; vehicle->applyEngineForce(engineForce, W_RL); vehicle->applyEngineForce(engineForce, W_RR); }
void CLPhysicsDemo::init(int preferredDevice, int preferredPlatform, bool useInterop) { InitCL(-1,-1,useInterop); #define CUSTOM_CL_INITIALIZATION #ifdef CUSTOM_CL_INITIALIZATION g_deviceCL = new adl::DeviceCL(); g_deviceCL->m_deviceIdx = g_device; g_deviceCL->m_context = g_cxMainContext; g_deviceCL->m_commandQueue = g_cqCommandQue; g_deviceCL->m_kernelManager = new adl::KernelManager; #else DeviceUtils::Config cfg; cfg.m_type = DeviceUtils::Config::DEVICE_CPU; g_deviceCL = DeviceUtils::allocate( TYPE_CL, cfg ); #endif //adl::Solver<adl::TYPE_CL>::allocate(g_deviceCL->allocate( m_data->m_linVelBuf = new adl::Buffer<btVector3>(g_deviceCL,MAX_CONVEX_BODIES_CL); m_data->m_angVelBuf = new adl::Buffer<btVector3>(g_deviceCL,MAX_CONVEX_BODIES_CL); m_data->m_bodyTimes = new adl::Buffer<float>(g_deviceCL,MAX_CONVEX_BODIES_CL); m_data->m_localShapeAABB = new adl::Buffer<btAABBHost>(g_deviceCL,MAX_CONVEX_SHAPES_CL); gLinVelMem = (cl_mem)m_data->m_linVelBuf->m_ptr; gAngVelMem = (cl_mem)m_data->m_angVelBuf->m_ptr; gBodyTimes = (cl_mem)m_data->m_bodyTimes->m_ptr; narrowphaseAndSolver = new btGpuNarrowphaseAndSolver(g_deviceCL); int maxObjects = btMax(256,MAX_CONVEX_BODIES_CL); int maxPairsSmallProxy = 32; btOverlappingPairCache* overlappingPairCache=0; m_data->m_Broadphase = new btGridBroadphaseCl(overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128, g_cxMainContext ,g_device,g_cqCommandQue, g_deviceCL); cl_program prog = btOpenCLUtils::compileCLProgramFromString(g_cxMainContext,g_device,interopKernelString,0,"",INTEROPKERNEL_SRC_PATH); g_integrateTransformsKernel = btOpenCLUtils::compileCLKernelFromString(g_cxMainContext, g_device,interopKernelString, "integrateTransformsKernel" ,0,prog); initFindPairs(gFpIO, g_cxMainContext, g_device, g_cqCommandQue, MAX_CONVEX_BODIES_CL); }
static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) { // It calculates possible 3 area constructed from random 4 points and returns the biggest one. btVector3 a[3],b[3]; a[0] = p0 - p1; a[1] = p0 - p2; a[2] = p0 - p3; b[0] = p2 - p3; b[1] = p1 - p3; b[2] = p1 - p2; //todo: Following 3 cross production can be easily optimized by SIMD. btVector3 tmp0 = a[0].cross(b[0]); btVector3 tmp1 = a[1].cross(b[1]); btVector3 tmp2 = a[2].cross(b[2]); return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); }
void Vehicle::Update(float dt) { const static float delta = 0.01f; //update steering float dif = targetSteering - steering; if(dif > delta) { steering += steeringIncrement*dt; steering = btMin(btMax(-1.f, steering), 1.f);//clamp } else if(dif < -delta) { steering -= steeringIncrement*dt; steering = btMin(btMax(-1.f, steering), 1.f);//clamp } vehicle->setSteeringValue(steering, W_FL); vehicle->setSteeringValue(steering, W_FR); }
btScalar CarEngine::Integrate(btScalar clutch_drag, btScalar clutch_angvel, btScalar dt) { btScalar rpm = GetRPM(); clutch_torque = clutch_drag; btScalar torque_limit = shaft.getMomentum(clutch_angvel) / dt; if ((clutch_torque > 0 && clutch_torque > torque_limit) || (clutch_torque < 0 && clutch_torque < torque_limit)) { clutch_torque = torque_limit; } stalled = (rpm < info.stall_rpm); //make sure the throttle is at least idling btScalar idle_position = info.idle_throttle + info.idle_throttle_slope * (rpm - info.start_rpm); if (throttle_position < idle_position) throttle_position = idle_position; //engine drive torque btScalar rev_limit = info.rpm_limit; if (rev_limit_exceeded) rev_limit -= 100.0; //tweakable rev_limit_exceeded = rpm > rev_limit; combustion_torque = info.GetTorque(throttle_position, rpm); //nitrous injection if (nos_mass > 0 && nos_boost_factor > 0) { btScalar boost = nos_boost_factor * info.nos_boost; combustion_torque += boost / shaft.ang_velocity; btScalar fuel_consumed = boost * info.fuel_rate * dt; btScalar nos_consumed = info.nos_fuel_ratio * fuel_consumed; nos_mass = btMax(btScalar(0), nos_mass - nos_consumed); } if (out_of_gas || rev_limit_exceeded || stalled) combustion_torque = 0.0; friction_torque = info.GetFrictionTorque(throttle_position, rpm); //try to model the static friction of the engine if (stalled) friction_torque *= 2; btScalar total_torque = combustion_torque + friction_torque + clutch_torque; shaft.applyMomentum(total_torque * dt); return clutch_torque; }
void CLPhysicsDemo::init(int preferredDevice, int preferredPlatform, bool useInterop) { InitCL(preferredDevice,preferredPlatform,useInterop); m_narrowphaseAndSolver = new btGpuNarrowphaseAndSolver(g_cxMainContext,g_device,g_cqCommandQue); g_narrowphaseAndSolver = m_narrowphaseAndSolver; //adl::Solver<adl::TYPE_CL>::allocate(g_deviceCL->allocate( m_data->m_linVelBuf = new btOpenCLArray<btVector3>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_BODIES_CL,false); m_data->m_angVelBuf = new btOpenCLArray<btVector3>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_BODIES_CL,false); m_data->m_bodyTimes = new btOpenCLArray<float>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_BODIES_CL,false); m_data->m_localShapeAABBGPU = new btOpenCLArray<btAABBHost>(g_cxMainContext,g_cqCommandQue,MAX_CONVEX_SHAPES_CL,false); m_data->m_localShapeAABBCPU = new btAlignedObjectArray<btAABBHost>; int maxObjects = btMax(256,MAX_CONVEX_BODIES_CL); int maxPairsSmallProxy = 32; if (useSapGpuBroadphase) { m_data->m_BroadphaseSap = new btGpuSapBroadphase(g_cxMainContext ,g_device,g_cqCommandQue);//overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128, } else { btOverlappingPairCache* overlappingPairCache=0; m_data->m_BroadphaseGrid = new btGridBroadphaseCl(overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128, g_cxMainContext ,g_device,g_cqCommandQue); } //g_cxMainContext ,g_device,g_cqCommandQue); m_data->m_pgsSolver = new btPgsJacobiSolver(); cl_program prog = btOpenCLUtils::compileCLProgramFromString(g_cxMainContext,g_device,interopKernelString,0,"",INTEROPKERNEL_SRC_PATH); g_integrateTransformsKernel = btOpenCLUtils::compileCLKernelFromString(g_cxMainContext, g_device,interopKernelString, "integrateTransformsKernel" ,0,prog); g_integrateTransformsKernel2 = btOpenCLUtils::compileCLKernelFromString(g_cxMainContext, g_device,interopKernelString, "integrateTransformsKernel2" ,0,prog); initFindPairs(gFpIO, g_cxMainContext, g_device, g_cqCommandQue, MAX_CONVEX_BODIES_CL); }
btHfFluidRigidCollisionConfiguration::btHfFluidRigidCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) :btDefaultCollisionConfiguration(constructionInfo) { void* mem; mem = btAlignedAlloc(sizeof(btHfFluidRigidCollisionAlgorithm::CreateFunc),16); m_hfFluidRigidConvexCreateFunc = new(mem) btHfFluidRigidCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btHfFluidRigidCollisionAlgorithm::CreateFunc),16); m_swappedHfFluidRigidConvexCreateFunc = new(mem) btHfFluidRigidCollisionAlgorithm::CreateFunc; m_swappedHfFluidRigidConvexCreateFunc->m_swapped = true; mem = btAlignedAlloc(sizeof(btHfFluidBuoyantShapeCollisionAlgorithm::CreateFunc),16); m_hfFluidBuoyantShapeCollisionCreateFunc = new(mem) btHfFluidBuoyantShapeCollisionAlgorithm::CreateFunc(m_simplexSolver, m_pdSolver); if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) { int curElemSize = m_collisionAlgorithmPool->getElementSize(); ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize0 = sizeof(btHfFluidRigidCollisionAlgorithm); int maxSize1 = 0; int maxSize2 = 0; int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); if (collisionAlgorithmMaxElementSize > curElemSize) { m_collisionAlgorithmPool->~btPoolAllocator(); btAlignedFree(m_collisionAlgorithmPool); void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } } }
btScalar Tire::getSqueal() const { btScalar squeal = 0; if (vx * vx > btScalar(1E-2) && slip * slip > btScalar(1E-6)) { btScalar vx_body = vx / slip; btScalar vx_ideal = ideal_slip * vx_body; btScalar vy_ideal = ideal_slip_angle * vx_body; //btTan(ideal_slip_angle) * vx_body; btScalar vx_squeal = btFabs(vx / vx_ideal); btScalar vy_squeal = btFabs(vy / vy_ideal); // start squeal at 80% of the ideal slide/slip, max out at 160% squeal = btScalar(1.25) * btMax(vx_squeal, vy_squeal) - 1; squeal = Clamp(squeal, btScalar(0), btScalar(1)); } return squeal; }
btScalar Tire::getSqueal() const { btScalar squeal = 0.0; if (vx * vx > 1E-2 && slide * slide > 1E-6) { btScalar vx_body = vx / slide; btScalar vx_ideal = ideal_slide * vx_body; btScalar vy_ideal = btTan(-ideal_slip / 180 * M_PI) * vx_body; btScalar vx_squeal = btFabs(vx / vx_ideal); btScalar vy_squeal = btFabs(vy / vy_ideal); // start squeal at 80% of the ideal slide/slip, max out at 160% squeal = 1.25 * btMax(vx_squeal, vy_squeal) - 1.0; btClamp(squeal, btScalar(0), btScalar(1)); } return squeal; }
btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, const int* triangles, int ntriangles, bool randomizeConstraints) { int maxidx=0; int i,j,ni; for(i=0,ni=ntriangles*3;i<ni;++i) { maxidx=btMax(triangles[i],maxidx); } ++maxidx; btAlignedObjectArray<bool> chks; btAlignedObjectArray<btVector3> vtx; chks.resize(maxidx*maxidx,false); vtx.resize(maxidx); for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3) { vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); } btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0); for( i=0,ni=ntriangles*3;i<ni;i+=3) { const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) for(int j=2,k=0;k<3;j=k++) { if(!chks[IDX(idx[j],idx[k])]) { chks[IDX(idx[j],idx[k])]=true; chks[IDX(idx[k],idx[j])]=true; psb->appendLink(idx[j],idx[k]); } } #undef IDX psb->appendFace(idx[0],idx[1],idx[2]); } if (randomizeConstraints) { psb->randomizeConstraints(); } return(psb); }
GL_ShapeDrawer::ShapeCache* GL_ShapeDrawer::cache(btConvexShape* shape) { ShapeCache* sc=(ShapeCache*)shape->getUserPointer(); if(!sc) { sc=new(btAlignedAlloc(sizeof(ShapeCache),16)) ShapeCache(shape); sc->m_shapehull.buildHull(shape->getMargin()); m_shapecaches.push_back(sc); shape->setUserPointer(sc); /* Build edges */ const int ni=sc->m_shapehull.numIndices(); const int nv=sc->m_shapehull.numVertices(); const unsigned int* pi=sc->m_shapehull.getIndexPointer(); const btVector3* pv=sc->m_shapehull.getVertexPointer(); btAlignedObjectArray<ShapeCache::Edge*> edges; sc->m_edges.reserve(ni); edges.resize(nv*nv,0); for(int i=0;i<ni;i+=3) { const unsigned int* ti=pi+i; const btVector3 nrm=btCross(pv[ti[1]]-pv[ti[0]],pv[ti[2]]-pv[ti[0]]).normalized(); for(int j=2,k=0;k<3;j=k++) { const unsigned int a=ti[j]; const unsigned int b=ti[k]; ShapeCache::Edge*& e=edges[btMin(a,b)*nv+btMax(a,b)]; if(!e) { sc->m_edges.push_back(ShapeCache::Edge()); e=&sc->m_edges[sc->m_edges.size()-1]; e->n[0]=nrm;e->n[1]=-nrm; e->v[0]=a;e->v[1]=b; } else { e->n[1]=nrm; } } } } return(sc); }
static void debugDrawPhase(const btBatchedConstraints* bc, btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies, int iPhase, const btVector3& color0, const btVector3& color1, const btVector3& offset) { BT_PROFILE("debugDrawPhase"); if (bc && bc->m_debugDrawer && iPhase < bc->m_phases.size()) { const btBatchedConstraints::Range& phase = bc->m_phases[iPhase]; for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) { float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1)); btVector3 col = lerp(color0, color1, tt); debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset); } } }
void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { #ifndef __SPU__ if (m_useSolveConstraintObsolete) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1; bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); } } // apply motor if (m_bMotorEnabled) { // compute current and predicted transforms btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); btTransform trAPred; trAPred.setIdentity(); btVector3 zerovec(0,0,0); btTransformUtil::integrateTransform( trACur, zerovec, omegaA, timeStep, trAPred); btTransform trBPred; trBPred.setIdentity(); btTransformUtil::integrateTransform( trBCur, zerovec, omegaB, timeStep, trBPred); // compute desired transforms in world btTransform trPose(m_qTarget); btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); btTransform trADes = trBPred * trABDes; btTransform trBDes = trAPred * trABDes.inverse(); // compute desired omegas in world btVector3 omegaADes, omegaBDes; btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); // compute delta omegas btVector3 dOmegaA = omegaADes - omegaA; btVector3 dOmegaB = omegaBDes - omegaB; // compute weighted avg axis of dOmega (weighting based on inertias) btVector3 axisA, axisB; btScalar kAxisAInv = 0, kAxisBInv = 0; if (dOmegaA.length2() > SIMD_EPSILON) { axisA = dOmegaA.normalized(); kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); } if (dOmegaB.length2() > SIMD_EPSILON) { axisB = dOmegaB.normalized(); kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); } btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; static bool bDoTorque = true; if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) { avgAxis.normalize(); kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); btScalar kInvCombined = kAxisAInv + kAxisBInv; btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / (kInvCombined * kInvCombined); if (m_maxMotorImpulse >= 0) { btScalar fMaxImpulse = m_maxMotorImpulse; if (m_bNormalizedMotorStrength) fMaxImpulse = fMaxImpulse/kAxisAInv; btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; btScalar newUnclampedMag = newUnclampedAccImpulse.length(); if (newUnclampedMag > fMaxImpulse) { newUnclampedAccImpulse.normalize(); newUnclampedAccImpulse *= fMaxImpulse; impulse = newUnclampedAccImpulse - m_accMotorImpulse; } m_accMotorImpulse += impulse; } btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); } } else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { btVector3 relVelAxis = relVel.normalized(); btScalar m_kDamping = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); btVector3 impulse = m_damping * m_kDamping * relVel; btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); } } // joint limits { ///solve angular part btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); // solve swing limit if (m_solveSwingLimit) { btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); if (relSwingVel > 0) amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; btScalar impulseMag = amplitude * m_kSwing; // Clamp the accumulated impulse btScalar temp = m_accSwingLimitImpulse; m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accSwingLimitImpulse - temp; btVector3 impulse = m_swingAxis * impulseMag; // don't let cone response affect twist // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) { btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; impulse = impulseNoTwistCouple; } impulseMag = impulse.length(); btVector3 noTwistSwingAxis = impulse / impulseMag; bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); } // solve twist limit if (m_solveTwistLimit) { btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; btScalar impulseMag = amplitude * m_kTwist; // Clamp the accumulated impulse btScalar temp = m_accTwistLimitImpulse; m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accTwistLimitImpulse - temp; // btVector3 impulse = m_twistAxis * impulseMag; bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); } } } #else btAssert(0); #endif //__SPU__ }
btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) { void* mem = NULL; if (constructionInfo.m_useEpaPenetrationAlgorithm) { mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; }else { mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; } //default CreationFunctions, filling the m_doubleDispatch table mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver); mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; m_boxSphereCF->m_swapped = true; #endif //USE_BUGGY_SPHERE_BOX_ALGORITHM mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF->m_swapped = true; mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; //convex versus plane mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; m_planeConvexCF->m_swapped = true; ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm); int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); if (constructionInfo.m_persistentManifoldPool) { m_ownsPersistentManifoldPool = false; m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; } else { m_ownsPersistentManifoldPool = true; void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); } collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0; if (constructionInfo.m_collisionAlgorithmPool) { m_ownsCollisionAlgorithmPool = false; m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; } else { m_ownsCollisionAlgorithmPool = true; void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } }
void btHingeConstraint::solveConstraint(btScalar timeStep) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); } } { ///solve angular part // get axes in world space btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction btScalar relaxation = btScalar(1.); btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; } //solve angular positional correction btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); angularError *= (btScalar(1.)/denom2) * relaxation; } m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); m_rbB.applyTorqueImpulse(velrelOrthog-angularError); // solve limit if (m_solveLimit) { btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; btScalar impulseMag = amplitude * m_kHinge; // Clamp the accumulated impulse btScalar temp = m_accLimitImpulse; m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); impulseMag = m_accLimitImpulse - temp; btVector3 impulse = axisA * impulseMag * m_limitSign; m_rbA.applyTorqueImpulse(impulse); m_rbB.applyTorqueImpulse(-impulse); } } //apply motor if (m_enableAngularMotor) { //todo: add limits too btVector3 angularLimit(0,0,0); btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; btVector3 motorImp = clippedMotorImpulse * axisA; m_rbA.applyTorqueImpulse(motorImp+angularLimit); m_rbB.applyTorqueImpulse(-motorImp-angularLimit); } } }
void btConeTwistConstraint::solveConstraint(btScalar timeStep) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); btScalar damping = btScalar(1.); //linear part if (!m_angularOnly) { btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); } } { ///solve angular part const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); // solve swing limit if (m_solveSwingLimit) { btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); btScalar impulseMag = amplitude * m_kSwing; // Clamp the accumulated impulse btScalar temp = m_accSwingLimitImpulse; m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accSwingLimitImpulse - temp; btVector3 impulse = m_swingAxis * impulseMag; m_rbA.applyTorqueImpulse(impulse); m_rbB.applyTorqueImpulse(-impulse); } // solve twist limit if (m_solveTwistLimit) { btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); btScalar impulseMag = amplitude * m_kTwist; // Clamp the accumulated impulse btScalar temp = m_accTwistLimitImpulse; m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accTwistLimitImpulse - temp; btVector3 impulse = m_twistAxis * impulseMag; m_rbA.applyTorqueImpulse(impulse); m_rbB.applyTorqueImpulse(-impulse); } } }
void Vehicle::init( const VehicleInfo & info, const btVector3 & position, const btQuaternion & rotation, World & world) { transform.setRotation(rotation); transform.setOrigin(position); body = new FractureBody(info.body); body->setCenterOfMassTransform(transform); body->setActivationState(DISABLE_DEACTIVATION); body->setContactProcessingThreshold(0.0); body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); world.addRigidBody(body); world.addAction(this); this->world = &world; aero_device.resize(info.aerodevice.size()); for (int i = 0; i < info.aerodevice.size(); ++i) { aero_device[i] = AeroDevice(info.aerodevice[i]); } antiroll.resize(info.antiroll.size()); for (int i = 0; i < info.antiroll.size(); ++i) { antiroll[i] = info.antiroll[i]; } differential.resize(info.differential.size()); wheel.resize(info.wheel.size()); wheel_contact.resize(wheel.size()); diff_joint.resize(differential.size()); clutch_joint.resize(differential.size() + 1); motor_joint.resize(wheel.size() * 2 + 1); for (int i = 0; i < wheel.size(); ++i) { wheel[i].init(info.wheel[i], world, *body); maxangle = btMax(maxangle, wheel[i].suspension.getMaxSteeringAngle()); } for (int i = 0; i < differential.size(); ++i) { Shaft * shaft_a = LinkShaft(info.differential_link_a[i], wheel, differential); Shaft * shaft_b = LinkShaft(info.differential_link_b[i], wheel, differential); differential[i].init(info.differential[i], *shaft_a, *shaft_b); } Shaft * shaft_t = LinkShaft(info.transmission_link, wheel, differential); transmission.init(info.transmission, *shaft_t); clutch.init(info.clutch); engine.init(info.engine); // position is the center of a 2 x 4 x 1 meter box on track surface // move car to fit bounding box front lower edge of the position box btVector3 bmin, bmax; body->getCollisionShape()->getAabb(btTransform::getIdentity(), bmin, bmax); btVector3 fwd = body->getCenterOfMassTransform().getBasis().getColumn(1); btVector3 up = body->getCenterOfMassTransform().getBasis().getColumn(2); btVector3 fwd_offset = fwd * (2.0 - bmax.y()); btVector3 up_offset = -up * (0.5 + bmin.z()); setPosition(body->getCenterOfMassPosition() + up_offset + fwd_offset); //alignWithGround(); // init constants calculateFrictionCoefficient(lon_friction_coeff, lat_friction_coeff); maxspeed = calculateMaxSpeed(); width = (bmax - bmin).dot(direction::right); }
void ObjectMotionState::setRestitution(float restitution) { _restitution = btMax(btMin(fabsf(restitution), 1.0f), 0.0f); }
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { ///for backwards compatibility during the transition to 'getInfo/getInfo2' if (m_useSolveConstraintObsolete) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btScalar tau = btScalar(0.3); //linear part if (!m_angularOnly) { btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1,vel2; bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) { const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); } } { ///solve angular part // get axes in world space btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); btVector3 angVelA; bodyA.getAngularVelocity(angVelA); btVector3 angVelB; bodyB.getAngularVelocity(angVelB); btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); //angularError *= (btScalar(1.)/denom2) * relaxation; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); } // solve limit if (m_solveLimit) { btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; btScalar impulseMag = amplitude * m_kHinge; // Clamp the accumulated impulse btScalar temp = m_accLimitImpulse; m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); impulseMag = m_accLimitImpulse - temp; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); } } //apply motor if (m_enableAngularMotor) { //todo: add limits too btVector3 angularLimit(0,0,0); btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; // accumulated impulse clipping: btScalar fMaxImpulse = m_maxMotorImpulse; btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse; btScalar clippedMotorImpulse = unclippedMotorImpulse; if (newAccImpulse > fMaxImpulse) { newAccImpulse = fMaxImpulse; clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; } else if (newAccImpulse < -fMaxImpulse) { newAccImpulse = -fMaxImpulse; clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; } m_accMotorImpulse += clippedMotorImpulse; bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); } } } }
btVector3 Tire::getForce( btScalar normal_force, btScalar friction_coeff, btScalar inclination, btScalar ang_velocity, btScalar lon_velocity, btScalar lat_velocity) { if (normal_force < 1E-3 || friction_coeff < 1E-3) { slide = slip = 0; ideal_slide = ideal_slip = 1; fx = fy = fz = mz = 0; vx = vy = 0; return btVector3(0, 0, 0); } // pacejka in kN normal_force = normal_force * 1E-3; // limit input btClamp(normal_force, btScalar(0), btScalar(max_load)); btClamp(inclination, -max_camber, max_camber); // get ideal slip ratio btScalar sigma_hat(0); btScalar alpha_hat(0); getSigmaHatAlphaHat(normal_force, sigma_hat, alpha_hat); btScalar Fz = normal_force; btScalar gamma = inclination; // positive when tire top tilts to the right, viewed from rear btScalar denom = btMax(btFabs(lon_velocity), btScalar(1E-3)); btScalar lon_slip_velocity = lon_velocity - ang_velocity; btScalar sigma = -lon_slip_velocity / denom; // longitudinal slip: negative in braking, positive in traction btScalar tan_alpha = lat_velocity / denom; btScalar alpha = -atan(tan_alpha) * 180.0 / M_PI; // sideslip angle: positive in a right turn(opposite to SAE tire coords) btScalar max_Fx(0), max_Fy(0), max_Mz(0); // combining method 1: beckman method for pre-combining longitudinal and lateral forces // seems to be a variant of Bakker 1987: // refined Kamm Circle for non-isotropic tire characteristics // and slip normalization to guarantee simultaneous sliding btScalar s = sigma / sigma_hat; btScalar a = alpha / alpha_hat; btScalar rho = btMax(btSqrt(s * s + a * a), btScalar(1E-4)); // normalized slip btScalar Fx = (s / rho) * PacejkaFx(rho * sigma_hat, Fz, friction_coeff, max_Fx); btScalar Fy = (a / rho) * PacejkaFy(rho * alpha_hat, Fz, gamma, friction_coeff, max_Fy); // Bakker (with unknown factor e(rho) to get the correct direction for large slip): // btScalar Fx = -s * PacejkaFx(rho * sigma_hat, Fz, friction_coeff, max_Fx); // btScalar Fy = -a * e(rho) * PacejkaFy(rho * alpha_hat, Fz, gamma, friction_coeff, max_Fy); /* // combining method 2: orangutan btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx); btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy); btScalar one = (sigma < 0.0) ? -1.0 : 1.0; btScalar pure_sigma = sigma / (one + sigma); btScalar pure_alpha = -tan_alpha / (one + sigma); btScalar pure_combined = sqrt(pure_sigma * pure_sigma + pure_alpha * pure_alpha); btScalar kappa_combined = pure_combined / (1.0 - pure_combined); btScalar alpha_combined = atan((one + sigma) * pure_combined); btScalar Flimitx = PacejkaFx(kappa_combined, Fz, friction_coeff, max_Fx); btScalar Flimity = PacejkaFy(alpha_combined * 180.0 / M_PI, Fz, gamma, friction_coeff, max_Fy); btScalar x = fabs(Fx) / (fabs(Fx) + fabs(Fy)); btScalar y = 1.0 - x; btScalar Flimit = (fabs(x * Flimitx) + fabs(y * Flimity)); btScalar Fmag = sqrt(Fx * Fx + Fy * Fy); if (Fmag > Flimit) { btScalar scale = Flimit / Fmag; Fx *= scale; Fy *= scale; } */ /* // combining method 3: traction circle btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx); btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy); // determine to what extent the tires are long (x) gripping vs lat (y) gripping // 1.0 when Fy is zero, 0.0 when Fx is zero btScalar longfactor = 1.0; btScalar combforce = btFabs(Fx) + btFabs(Fy); if (combforce > 1) longfactor = btFabs(Fx) / combforce; // determine the maximum force for this amount of long vs lat grip by linear interpolation btScalar maxforce = btFabs(max_Fx) * longfactor + (1.0 - longfactor) * btFabs(max_Fy); // scale down forces to fit into the maximum if (combforce > maxforce) { Fx *= maxforce / combforce; Fy *= maxforce / combforce; } */ /* // combining method 4: traction ellipse (prioritize Fx) // issue: assumes that adhesion limits are fully reached for combined forces btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx); btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy); if (Fx < max_Fx) { Fy = Fy * sqrt(1.0 - (Fx / max_Fx) * (Fx / max_Fx)); } else { Fx = max_Fx; Fy = 0; } */ /* // combining method 5: traction ellipse (prioritize Fy) // issue: assumes that adhesion limits are fully reached for combined forces btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx); btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy); if (Fy < max_Fy) { Fx = Fx * sqrt(1.0 - (Fy / max_Fy) * (Fy / max_Fy)); } else { Fy = max_Fy; Fx = 0; } */ /* // no combining btScalar Fx = PacejkaFx(sigma, Fz, friction_coeff, max_Fx); btScalar Fy = PacejkaFy(alpha, Fz, gamma, friction_coeff, max_Fy); */ btScalar Mz = PacejkaMz(sigma, alpha, Fz, gamma, friction_coeff, max_Mz); camber = inclination; slide = sigma; slip = alpha; ideal_slide = sigma_hat; ideal_slip = alpha_hat; fx = Fx; fy = Fy; fz = Fz; mz = Mz; vx = lon_slip_velocity; vy = lat_velocity; // Fx positive during traction, Fy positive in a right turn, Mz positive in a left turn return btVector3(Fx, Fy, Mz); }
void Camera::Translate(const Vector3& vec) { Vector3 forwards; float z = vec.z * -1.0f; if (m_type == CameraType::FIRST_PERSON) { forwards = WORLD_Y_AXIS.Cross(m_localXAxis); forwards.Normalize(); } else { forwards = m_viewVector; } auto& current = m_transform.Position; //auto& current = btt.getOrigin(); if (m_collider) { //btVector3 after = current; Vector3 after = current; after += m_localXAxis.AsBtVector3() * vec.x; after += WORLD_Y_AXIS.AsBtVector3() * vec.y; after += forwards.AsBtVector3() * z; btTransform from, to; from.setIdentity(); to.setIdentity(); from.setOrigin(current.AsBtVector3()); to.setOrigin(after.AsBtVector3()); btCollisionWorld::ClosestConvexResultCallback cb(current.AsBtVector3(), after.AsBtVector3()); cb.m_collisionFilterMask = btBroadphaseProxy::DefaultFilter | btBroadphaseProxy::StaticFilter; btConvexShape* cs = static_cast<btConvexShape*>(m_collider->GetRigidBody()->getCollisionShape()); m_physicsWorld->ConvexSweepTest(cs, from, to, cb); if (cb.hasHit() && cb.m_hitCollisionObject != m_collider->GetRigidBody()) { /*if () { printf("\nHit Self"); }*/ float epsilon = 0.0001; float min = btMax(epsilon, cb.m_closestHitFraction); current += -forwards * 0.1; btVector3 newPos = current.AsBtVector3(); //newPos.setInterpolate3(current.AsBtVector3(), after.AsBtVector3(), min); m_transform.Position = Vector3(newPos); } else { m_transform.Position = after; } } else { current += m_localXAxis.AsBtVector3() * vec.x; current += WORLD_Y_AXIS.AsBtVector3() * vec.y; current += forwards.AsBtVector3() * z; m_transform.Position = current; } if (m_collider) { btTransform btt; btt.setRotation(m_transform.Orientation.AsBtQuaternion()); btt.setOrigin(m_transform.Position.AsBtVector3()); m_collider->SetWorldTransform(btt); } }
void ObjectMotionState::setAngularDamping(float damping) { _angularDamping = btMax(btMin(fabsf(damping), 1.0f), 0.0f); }
void ObjectMotionState::setLinearDamping(float damping) { _linearDamping = btMax(btMin(fabsf(damping), 1.0f), 0.0f); }
void Vehicle::SetTargetSteering(float steering) { steering = btMin(btMax(-1.f, steering), 1.f);//clamp targetSteering = steering * steeringClamp; }
void ObjectMotionState::setFriction(float friction) { _friction = btMax(btMin(fabsf(friction), MAX_FRICTION), 0.0f); }