本文整理汇总了C++中btFabs函数的典型用法代码示例。如果您正苦于以下问题:C++ btFabs函数的具体用法?C++ btFabs怎么用?C++ btFabs使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了btFabs函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: btFabs
btScalar Tire::PacejkaMz(btScalar sigma, btScalar alpha, btScalar Fz, btScalar gamma, btScalar friction_coeff, btScalar & max_Mz) const
{
const std::vector<btScalar> & c = aligning;
btScalar C = c[0];
// peak factor
btScalar D = (c[1] * Fz + c[2]) * Fz;
// peak factor 1993
// D = D * (1 - c[18] * gamma * gamma);
// slope at origin
btScalar BCD = (c[3] * Fz + c[4]) * Fz * (1.0 - c[6] * btFabs(gamma)) * exp (-c[5] * Fz);
// stiffness factor
btScalar B = BCD / (C * D);
// curvature factor
btScalar E = (c[7] * Fz * Fz + c[8] * Fz + c[9]) * (1.0 - c[10] * btFabs(gamma));
// curvature factor 1993
// E = (c[7] * Fz * Fz + c[8] * Fz + c[9]) * (1.0 - (c[19] * gamma + c[20]) * sgn(S)) / (1.0 - c[10] * btFabs(gamma));
// horizontal shift
btScalar Sh = c[11] * gamma + c[12] * Fz + c[13];
// horizontal shift 1993
// Sh = c[11] * Fz + c[12] + c[13] * gamma;
// vertical shift
btScalar Sv = (c[14] * Fz * Fz + c[15] * Fz) * gamma + c[16] * Fz + c[17];
// vertical shift 1993
// Sv = c[14] * Fz + c[15] + (c[16] * Fz * Fz + c[17] * Fz) * gamma;
// composite
btScalar S = alpha + Sh;
// self-aligning torque
btScalar Mz = D * sin(c[0] * atan(B * S - E * (B * S - atan(B * S)))) + Sv;
// scale by surface friction
Mz = Mz * friction_coeff;
max_Mz = (D + Sv) * friction_coeff;
btAssert(Mz == Mz);
return Mz;
}
开发者ID:wyuka,项目名称:vdrift,代码行数:49,代码来源:tire.cpp
示例2: btTan
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;
}
开发者ID:wyuka,项目名称:vdrift,代码行数:16,代码来源:tire.cpp
示例3: btPlaneSpace1
void btPlaneSpace1 (const float4* n, float4* p, float4* q)
{
if (btFabs(n->z) > SIMDSQRT12) {
// choose p in y-z plane
btScalar a = n->y*n->y + n->z*n->z;
btScalar k = btRecipSqrt (a);
p->x = 0;
p->y = -n->z*k;
p->z = n->y*k;
// set q = n x p
q->x = a*k;
q->y = -n->x*p->z;
q->z = n->x*p->y;
}
else {
// choose p in x-y plane
btScalar a = n->x*n->x + n->y*n->y;
btScalar k = btRecipSqrt (a);
p->x = -n->y*k;
p->y = n->x*k;
p->z = 0;
// set q = n x p
q->x = -n->z*p->y;
q->y = n->z*p->x;
q->z = a*k;
}
}
开发者ID:cf2013,项目名称:experiments,代码行数:27,代码来源:Solver.cpp
示例4: internalProcessTriangleIndex
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{
(void)triangleIndex;
(void)partId;
btMatrix3x3 i;
btVector3 a = triangle[0] - center;
btVector3 b = triangle[1] - center;
btVector3 c = triangle[2] - center;
btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
for (int j = 0; j < 3; j++)
{
for (int k = 0; k <= j; k++)
{
i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
}
}
btScalar i00 = -i[0][0];
btScalar i11 = -i[1][1];
btScalar i22 = -i[2][2];
i[0][0] = i11 + i22;
i[1][1] = i22 + i00;
i[2][2] = i00 + i11;
sum[0] += i[0];
sum[1] += i[1];
sum[2] += i[2];
}
开发者ID:ilpincy,项目名称:argos3,代码行数:26,代码来源:btConvexTriangleMeshShape.cpp
示例5: btFabs
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;
}
开发者ID:Timo6,项目名称:vdrift,代码行数:16,代码来源:tire.cpp
示例6: btScalar
void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
{
const btVector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
const btVector3& N3 = planeEquations[k];
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > btScalar(0.0001) ) &&
( n3n1.length2() > btScalar(0.0001) ) &&
( n1n2.length2() > btScalar(0.0001) ) )
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.000001))
{
quotient = btScalar(-1.) / quotient;
n2n3 *= N1[3];
n3n1 *= N2[3];
n1n2 *= N3[3];
btVector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
}
}
}
}
}
}
开发者ID:Cassie90,项目名称:ClanLib,代码行数:56,代码来源:btGeometryUtil.cpp
示例7: btAssert
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
int j;
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (j=0;j<numPoolConstraints;j++)
{
const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
}
//do a callback here?
}
}
numPoolConstraints = m_multiBodyNonContactConstraints.size();
#if 0
//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
for (int i=0;i<numPoolConstraints;i++)
{
const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
btJointFeedback* fb = constr->getJointFeedback();
if (fb)
{
fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
}
constr->internalSetAppliedImpulse(c.m_appliedImpulse);
if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
{
constr->setEnabled(false);
}
}
#endif
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
}
开发者ID:03050903,项目名称:libgdx,代码行数:56,代码来源:btMultiBodyConstraintSolver.cpp
示例8: resolveSingleBilateral
//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
(void)timeStep;
(void)distance;
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
{
impulse = btScalar(0.);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = btScalar(0.2);
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
开发者ID:jordanlittlefair,项目名称:Foundation,代码行数:55,代码来源:btContactConstraint.cpp
示例9: btShortestAngleUpdate
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
{
btScalar tol(0.3);
btScalar result = btShortestAngularDistance(accAngle, curAngle);
if (btFabs(result) > tol)
return curAngle;
else
return accAngle + result;
return curAngle;
}
开发者ID:Aatch,项目名称:bullet3,代码行数:12,代码来源:btHingeConstraint.cpp
示例10: getGeometryIndex
void CubeGeometry::setColorBase(Node *node)
{
unsigned long index = getGeometryIndex(node);
if(m_VertexData)
{
btVector4 c(btFabs(node->getColorBase().x()),
btFabs(node->getColorBase().y()),
btFabs(node->getColorBase().z()),
btFabs(node->getColorBase().w()));
m_VertexData[index].blf.color = c;
m_VertexData[index].brf.color = c;
m_VertexData[index].tlf.color = c;
m_VertexData[index].trf.color = c;
m_VertexData[index].blb.color = c;
m_VertexData[index].brb.color = c;
m_VertexData[index].tlb.color = c;
m_VertexData[index].trb.color = c;
}
}
开发者ID:njligames,项目名称:verizontest,代码行数:22,代码来源:CubeGeometry.cpp
示例11: PacejkaFx
void Tire::findSigmaHatAlphaHat(
btScalar load,
btScalar & output_sigmahat,
btScalar & output_alphahat,
int iterations)
{
btScalar Fz = load;
btScalar Fz0 = nominal_load;
btScalar dFz = (Fz - Fz0) / Fz0;
btScalar camber = 0.0;
btScalar mu = 1.0;
btScalar Dy, BCy, Shf; // unused
btScalar Fxmax = 0.0;
btScalar smax = 2.0;
for (btScalar s = -smax; s < smax; s += 2 * smax / iterations)
{
btScalar Fx = PacejkaFx(s, Fz, dFz, mu);
if (Fx > Fxmax)
{
output_sigmahat = btFabs(s);
Fxmax = Fx;
}
}
btScalar Fymax = 0.0;
btScalar amax = 30.0 * (M_PI / 180.0);
for (btScalar a = -amax; a < amax; a += 2 * amax / iterations)
{
btScalar Fy = PacejkaFy(a, camber, Fz, dFz, mu, Dy, BCy, Shf);
if (Fy > Fymax)
{
output_alphahat = btFabs(a);
Fymax = Fy;
}
}
}
开发者ID:Timo6,项目名称:vdrift,代码行数:37,代码来源:tire.cpp
示例12: sin
btScalar Tire::PacejkaFy(btScalar alpha, btScalar Fz, btScalar gamma, btScalar friction_coeff, btScalar & max_Fy) const
{
const std::vector<btScalar> & a = lateral;
// shape factor
btScalar C = a[0];
// peak factor
btScalar D = (a[1] * Fz + a[2]) * Fz;
// peak factor 1993
// D = D * (1 - a[15] * gamma * gamma);
// slope at origin
btScalar BCD = a[3] * sin(2.0 * atan(Fz / a[4])) * (1.0 - a[5] * btFabs(gamma));
// stiffness factor
btScalar B = BCD / (C * D);
// curvature factor
btScalar E = a[6] * Fz + a[7];
// curvature factor 1993
// E = E * (1 - (a[16] * gamma + a[17]) * sgn(alpha + Sh));
// horizontal shift
btScalar Sh = a[8] * gamma + a[9] * Fz + a[10];
// horizontal shift 1993
// Sh = a[8] * Fz + a[9] + a[10] * gamma;
// vertical shift
btScalar Sv = ((a[11] * Fz + a[12]) * gamma + a[13]) * Fz + a[14];
// vertical shift 1993
// Sv = a[11] * Fz + a[12] + (a[13] * Fz * Fz + a[14] * Fz) * gamma;
// composite
btScalar S = alpha + Sh;
// lateral force
btScalar Fy = D * sin(C * atan(B * S - E * (B * S - atan(B * S)))) + Sv;
// scale by surface friction
Fy = Fy * friction_coeff;
max_Fy = (D + Sv) * friction_coeff;
return Fy;
}
开发者ID:wyuka,项目名称:vdrift,代码行数:49,代码来源:tire.cpp
示例13: btScalar
void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
{
// it is assumed that calculateTransforms() have been called before this call
int i;
btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
for(i = 0; i < 3; i++)
{
if(m_springEnabled[i])
{
// get current position of constraint
btScalar currPos = m_calculatedLinearDiff[i];
// calculate difference
btScalar delta = currPos - m_equilibriumPoint[i];
// spring force is (delta * m_stiffness) according to Hooke's Law
btScalar force = delta * m_springStiffness[i];
btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
m_linearLimits.m_targetVelocity[i] = velFactor * force;
m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
}
}
for(i = 0; i < 3; i++)
{
if(m_springEnabled[i + 3])
{
// get current position of constraint
btScalar currPos = m_calculatedAxisAngleDiff[i];
// calculate difference
btScalar delta = currPos - m_equilibriumPoint[i+3];
// spring force is (-delta * m_stiffness) according to Hooke's Law
btScalar force = -delta * m_springStiffness[i+3];
btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
m_angularLimits[i].m_targetVelocity = velFactor * force;
m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
}
}
}
开发者ID:Akira-Hayasaka,项目名称:ofxBulletPhysics,代码行数:36,代码来源:btGeneric6DofSpringConstraint.cpp
示例14: getVerticesInsidePlanes
void Voronoi::getVerticesInsidePlanes(const vector<btVector3>& planes, vector<btVector3>& verticesOut, std::set<int>& planeIndicesOut)
{
// Based on btGeometryUtil.cpp (Gino van den Bergen / Erwin Coumans)
verticesOut.resize(0);
planeIndicesOut.clear();
const int numPlanes = planes.size();
int i, j, k, l;
for (i=0;i<numPlanes;i++)
{
const btVector3& N1 = planes[i];
for (j=i+1;j<numPlanes;j++)
{
const btVector3& N2 = planes[j];
btVector3 n1n2 = N1.cross(N2);
if (n1n2.length2() > btScalar(0.0001))
{
for (k=j+1;k<numPlanes;k++)
{
const btVector3& N3 = planes[k];
btVector3 n2n3 = N2.cross(N3);
btVector3 n3n1 = N3.cross(N1);
if ((n2n3.length2() > btScalar(0.0001)) && (n3n1.length2() > btScalar(0.0001) ))
{
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.0001))
{
btVector3 potentialVertex = (n2n3 * N1[3] + n3n1 * N2[3] + n1n2 * N3[3]) * (btScalar(-1.) / quotient);
for (l=0; l<numPlanes; l++)
{
const btVector3& NP = planes[l];
if (btScalar(NP.dot(potentialVertex))+btScalar(NP[3]) > btScalar(0.000001))
break;
}
if (l == numPlanes)
{
// vertex (three plane intersection) inside all planes
verticesOut.push_back(potentialVertex);
planeIndicesOut.insert(i);
planeIndicesOut.insert(j);
planeIndicesOut.insert(k);
}
}
}
}
}
}
}
}
开发者ID:PuKoren,项目名称:voronoi-3d,代码行数:48,代码来源:Voronoi.cpp
示例15: btAssert
//bilateral constraint between two dynamic objects
void RaycastCar::resolveSingleBilateral(btRigidBody & body1,
const btVector3 & pos1,
btRigidBody & body2,
const btVector3 & pos2,
const btVector3 & normal,
btScalar & impulse)
{
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1f));
if (normalLenSqr > btScalar(1.1f))
{
impulse = btScalar(0.0f);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,
rel_pos2,
normal,
body1.getInvInertiaDiagLocal(),
body1.getInvMass(),
body2.getInvInertiaDiagLocal(),
body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.0f) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity
(body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose()*body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose()*body2.getAngularVelocity());
btScalar velocityImpulse = -1.0f * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
开发者ID:issakomi,项目名称:mmt,代码行数:39,代码来源:raycast_car.cpp
示例16: btAdjustInternalEdgeContacts
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
{
//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return;
btBvhTriangleMeshShape* trimesh = 0;
if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
}
else
{
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
}
}
if (trimesh==0)
return;
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;
int hash = btGetHash(partId0,index0);
btTriangleInfo* info = triangleInfoMapPtr->find(hash);
if (!info)
return;
btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
btVector3 v0,v1,v2;
tri_shape->getVertex(0,v0);
tri_shape->getVertex(1,v1);
tri_shape->getVertex(2,v2);
//btVector3 center = (v0+v1+v2)*btScalar(1./3.);
btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
btVector3 tri_normal;
tri_shape->calcNormal(tri_normal);
//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
btVector3 nearest;
btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
const btTransform& tr = colObj0->getWorldTransform();
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.normalize();//is this necessary?
// Get closest edge
int bestedge=-1;
btScalar disttobestedge=BT_LARGE_FLOAT;
//
// Edge 0 -> 1
if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
btScalar len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=0;
disttobestedge=len;
}
}
// Edge 1 -> 2
if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
btScalar len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=1;
disttobestedge=len;
}
}
// Edge 2 -> 0
if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
//.........这里部分代码省略.........
开发者ID:93i,项目名称:godot,代码行数:101,代码来源:btInternalEdgeUtility.cpp
示例17: getCalculatedTransformA
//.........这里部分代码省略.........
{
info->cfm[srow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
{
info->cfm[srow] = m_cfmLimLin;
}
if(lostop == histop)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
else
{ // high limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
if(bounce > btScalar(0.0))
{
btScalar vel = linVelA.dot(ax1);
vel -= linVelB.dot(ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimLin();
} // if(limit)
开发者ID:0302zq,项目名称:libgdx,代码行数:67,代码来源:btSliderConstraint.cpp
示例18: btScalar
void btConeTwistConstraint::calcAngleInfo()
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
m_twistAngle = twist;
// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
}
}
}
开发者ID:5432935,项目名称:awayphysics-core-fp11,代码行数:85,代码来源:btConeTwistConstraint.cpp
示例19: btAssert
//.........这里部分代码省略.........
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
开发者ID:joyfish,项目名称:GameThirdPartyLibs,代码行数:67,代码来源:btMultiBodyConstraint.cpp
示例20: btFabs
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{
int i;
// linear
btVector3 velA;
bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
btVector3 velB;
bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
btVector3 vel = velA - velB;
for(i = 0; i < 3; i++)
{
const btVector3& normal = m_jacLin[i].m_linearJointAxis;
btScalar rel_vel = normal.dot(vel);
// calculate positional error
btScalar depth = m_depth[i];
// get parameters
btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
// calcutate and apply impulse
btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
btVector3 impulse_vector = normal * normalImpulse;
//rbA.applyImpulse( impulse_vector, m_relPosA);
//rbB.applyImpulse(-impulse_vector, m_relPosB);
{
btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
}
if(m_poweredLinMotor && (!i))
{ // apply linear motor
if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
{
btScalar desiredMotorVel = m_targetLinMotorVelocity;
btScalar motor_relvel = desiredMotorVel + rel_vel;
normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
// clamp accumulated impulse
btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
if(new_acc > m_maxLinMotorForce)
{
new_acc = m_maxLinMotorForce;
}
btScalar del = new_acc - m_accumulatedLinMotorImpulse;
if(normalImpulse < btScalar(0.0))
{
normalImpulse = -del;
}
else
{
normalImpulse = del;
}
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
//rbA.applyImpulse( impulse_vector, m_relPosA);
//rbB.applyImpulse(-impulse_vector, m_relPosB);
{
btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
}
}
}
}
// angular
// get axes in world space
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
btVector3 angAorthog = angVelA - angVelAroundAxisA;
btVector3 angBorthog = angVelB - angVelAroundAxisB;
btVector3 velrelOrthog = angAorthog-angBorthog;
//solve orthogonal angular velocity correction
btScalar len = velrelOrthog.length();
btScalar orthorImpulseMag = 0.f;
if (len > btScalar(0.00001))
{
btVector3 normal = velrelOrthog.normalized();
btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
//.........这里部分代码省略.........
开发者ID:jinjoh,项目名称:NOOR,代码行数:101,代码来源:btSliderConstraint.cpp
注:本文中的btFabs函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论