本文整理汇总了C++中btAssert函数的典型用法代码示例。如果您正苦于以下问题:C++ btAssert函数的具体用法?C++ btAssert怎么用?C++ btAssert使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了btAssert函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: finalizeMultiDof
void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
return;
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
// row 0: the lower bound
jacobianA(0)[offsetA] = 1;
jacobianB(0)[offsetB] = m_gearRatio;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar erp = infoGlobal.m_erp;
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
for (int row=0;row<numRows;row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar auxVel = 0;
if (m_gearAuxLink>=0)
{
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
//btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}
开发者ID:bingjeff,项目名称:bullet3,代码行数:97,代码来源:btMultiBodyGearConstraint.cpp
示例2: btAssert
MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
{
btAssert(0);
(void)other;
return *this;
}
开发者ID:300833356COMP392,项目名称:DodgeTheMeteorShower,代码行数:6,代码来源:btMultiBodyDynamicsWorld.cpp
示例3: btAssert
void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
{
(void)index;
btAssert(m_useQuantization);
int curNodeSubPart=-1;
//get access info to trianglemesh data
const unsigned char *vertexbase = 0;
int numverts = 0;
PHY_ScalarType type = PHY_INTEGER;
int stride = 0;
const unsigned char *indexbase = 0;
int indexstride = 0;
int numfaces = 0;
PHY_ScalarType indicestype = PHY_INTEGER;
btVector3 triangleVerts[3];
btVector3 aabbMin,aabbMax;
const btVector3& meshScaling = meshInterface->getScaling();
int i;
for (i=endNode-1; i>=firstNode; i--)
{
btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
if (curNode.isLeafNode())
{
//recalc aabb from triangle data
int nodeSubPart = curNode.getPartId();
int nodeTriangleIndex = curNode.getTriangleIndex();
if (nodeSubPart != curNodeSubPart)
{
if (curNodeSubPart >= 0)
meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
curNodeSubPart = nodeSubPart;
btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
}
//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
for (int j=2; j>=0; j--)
{
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = btVector3(
graphicsbase[0]*meshScaling.getX(),
graphicsbase[1]*meshScaling.getY(),
graphicsbase[2]*meshScaling.getZ());
}
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
}
}
aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
aabbMin.setMin(triangleVerts[0]);
aabbMax.setMax(triangleVerts[0]);
aabbMin.setMin(triangleVerts[1]);
aabbMax.setMax(triangleVerts[1]);
aabbMin.setMin(triangleVerts[2]);
aabbMax.setMax(triangleVerts[2]);
quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
} else
{
//combine aabb from both children
btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
&m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
{
for (int i=0; i<3; i++)
{
curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
//.........这里部分代码省略.........
开发者ID:dreamsxin,项目名称:nawia,代码行数:101,代码来源:btOptimizedBvh.cpp
示例4: coordinates
/**
basic algorithm:
- convert input aabb to local coordinates (scale down and shift for local origin)
- convert input aabb to a range of heightfield grid points (quantize)
- iterate over all triangles in that subset of the grid
*/
void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
// scale down the input aabb's so they are in local (non-scaled) coordinates
btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
// account for local origin
localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int quantizedAabbMin[3];
int quantizedAabbMax[3];
quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
// expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i) {
quantizedAabbMin[i]--;
quantizedAabbMax[i]++;
}
int startX=0;
int endX=m_heightStickWidth-1;
int startJ=0;
int endJ=m_heightStickLength-1;
switch (m_upAxis)
{
case 0:
{
if (quantizedAabbMin[1]>startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
}
case 1:
{
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
};
case 2:
{
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1]>startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endJ)
endJ = quantizedAabbMax[1];
break;
}
default:
{
//need to get valid m_upAxis
btAssert(0);
}
}
for(int j=startJ; j<endJ; j++)
{
for(int x=startX; x<endX; x++)
{
btVector3 vertices[3];
if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision && !(j & 1)))
{
//first triangle
getVertex(x,j,vertices[0]);
getVertex(x+1,j,vertices[1]);
getVertex(x+1,j+1,vertices[2]);
callback->processTriangle(vertices,x,j);
//second triangle
// getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
getVertex(x+1,j+1,vertices[1]);
getVertex(x,j+1,vertices[2]);
callback->processTriangle(vertices,x,j);
} else
//.........这里部分代码省略.........
开发者ID:1005491398,项目名称:Threes,代码行数:101,代码来源:btHeightfieldTerrainShape.cpp
示例5: btAssert
void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const
{
btAssert(0);
}
开发者ID:1079395024,项目名称:Torque3D,代码行数:5,代码来源:btConvexHullShape.cpp
示例6: btAssert
void SpuContactManifoldCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
btAssert(0);
}
开发者ID:RyunosukeOno,项目名称:rayjack,代码行数:4,代码来源:SpuContactManifoldCollisionAlgorithm.cpp
示例7: normalInB
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
#endif
{
m_cachedSeparatingDistance = 0.f;
btScalar distance=btScalar(0.);
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 pointOnA,pointOnB;
btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB;
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset;
bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
btScalar marginA = m_marginA;
btScalar marginB = m_marginB;
gNumGjkChecks++;
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("inside gjk\n");
#endif
//for CCD we don't use margins
if (m_ignoreMargin)
{
marginA = btScalar(0.);
marginB = btScalar(0.);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("ignoring margin\n");
#endif
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue(0,1,0);
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
{
btScalar squaredDistance = BT_LARGE_FLOAT;
btScalar delta = btScalar(0.);
btScalar margin = marginA + marginB;
m_simplexSolver->reset();
for ( ; ; )
//while (true)
{
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
#if 1
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
#else
#ifdef __SPU__
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
#else
btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
#ifdef TEST_NON_VIRTUAL
btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
btAssert((pInAv-pInA).length() < 0.0001);
btAssert((qInBv-qInB).length() < 0.0001);
#endif //
#endif //__SPU__
#endif
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("got local supporting vertices\n");
#endif
if (check2d)
{
pWorld[2] = 0.f;
qWorld[2] = 0.f;
}
//.........这里部分代码省略.........
开发者ID:1005491398,项目名称:Threes,代码行数:101,代码来源:btGjkPairDetector.cpp
示例8: init
void OpenGL2Renderer::renderPhysicsWorld(int numObjects, btCollisionObject** objectArray)
{
init();
updateCamera();
if (1)
{
if(m_enableshadows)
{
glClear(GL_STENCIL_BUFFER_BIT);
glEnable(GL_CULL_FACE);
GLint err = glGetError();
btAssert(err==GL_NO_ERROR);
renderscene(0,numObjects,objectArray);
err = glGetError();
btAssert(err==GL_NO_ERROR);
glDisable(GL_LIGHTING);
glDepthMask(GL_FALSE);
glDepthFunc(GL_LEQUAL);
glEnable(GL_STENCIL_TEST);
glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
glFrontFace(GL_CCW);
glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
renderscene(1,numObjects,objectArray);
glFrontFace(GL_CW);
glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
renderscene(1,numObjects,objectArray);
glFrontFace(GL_CCW);
err = glGetError();
btAssert(err==GL_NO_ERROR);
glPolygonMode(GL_FRONT,GL_FILL);
glPolygonMode(GL_BACK,GL_FILL);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glEnable(GL_LIGHTING);
glDepthMask(GL_TRUE);
glCullFace(GL_BACK);
glFrontFace(GL_CCW);
glEnable(GL_CULL_FACE);
glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
glDepthFunc(GL_LEQUAL);
glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
glDisable(GL_LIGHTING);
renderscene(2,numObjects,objectArray);
glEnable(GL_LIGHTING);
glDepthFunc(GL_LESS);
glDisable(GL_STENCIL_TEST);
glDisable(GL_CULL_FACE);
err = glGetError();
btAssert(err==GL_NO_ERROR);
}
else
{
glDisable(GL_CULL_FACE);
renderscene(0,numObjects,objectArray);
}
int xOffset = 10;
int yStart = 20;
int yIncr = 20;
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
{
setOrthographicProjection();
//showProfileInfo(xOffset,yStart,yIncr);
#if 0//#ifdef USE_QUICKPROF
if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
//.........这里部分代码省略.........
开发者ID:cf2013,项目名称:experiments,代码行数:101,代码来源:OpenGL2Renderer.cpp
示例9: glGetError
void OpenGL2Renderer::updateCamera()
{
GLint err = glGetError();
btAssert(err==GL_NO_ERROR);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg
btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg
btQuaternion rot(m_cameraUp,razi);
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
err = glGetError();
btAssert(err==GL_NO_ERROR);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_openglViewportWidth == 0 && m_openglViewportHeight == 0)
return;
btScalar aspect;
btVector3 extents;
aspect = m_openglViewportWidth / (btScalar)m_openglViewportHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
} else
{
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
err = glGetError();
btAssert(err==GL_NO_ERROR);
}
开发者ID:cf2013,项目名称:experiments,代码行数:69,代码来源:OpenGL2Renderer.cpp
示例10: getNumSubParts
void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
(void)aabbMin;
(void)aabbMax;
int numtotalphysicsverts = 0;
int part, graphicssubparts = getNumSubParts();
const unsigned char * vertexbase;
const unsigned char * indexbase;
int indexstride;
PHY_ScalarType type;
PHY_ScalarType gfxindextype;
int stride, numverts, numtriangles;
int gfxindex;
btVector3 triangle[3];
btVector3 meshScaling = getScaling();
///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
for (part=0; part<graphicssubparts ; part++)
{
getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part);
numtotalphysicsverts+=numtriangles*3; //upper bound
///unlike that developers want to pass in double-precision meshes in single-precision Bullet build
///so disable this feature by default
///see patch http://code.google.com/p/bullet/issues/detail?id=213
switch (type)
{
case PHY_FLOAT:
{
float* graphicsbase;
switch (gfxindextype)
{
case PHY_INTEGER:
{
for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
{
unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
callback->internalProcessTriangleIndex(triangle, part, gfxindex);
}
break;
}
case PHY_SHORT:
{
for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
{
unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
callback->internalProcessTriangleIndex(triangle, part, gfxindex);
}
break;
}
case PHY_UCHAR:
{
for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
{
unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride);
graphicsbase = (float*)(vertexbase+tri_indices[0]*stride);
triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
graphicsbase = (float*)(vertexbase+tri_indices[1]*stride);
triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
graphicsbase = (float*)(vertexbase+tri_indices[2]*stride);
triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ());
callback->internalProcessTriangleIndex(triangle, part, gfxindex);
}
break;
}
default:
btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
}
break;
}
case PHY_DOUBLE:
{
double* graphicsbase;
switch (gfxindextype)
{
case PHY_INTEGER:
{
for (gfxindex=0; gfxindex<numtriangles; gfxindex++)
{
unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
graphicsbase = (double*)(vertexbase+tri_indices[0]*stride);
triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ());
//.........这里部分代码省略.........
开发者ID:ILOVEPIE,项目名称:Gmod-vphysics,代码行数:101,代码来源:btStridingMeshInterface.cpp
示例11: btAssert
void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
// transforms in world space
btTransform trA = transA*m_rbAFrame;
btTransform trB = transB*m_rbBFrame;
// pivot point
btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin();
#if 1
// difference between frames in WCS
btVector3 ofs = trB.getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
btScalar miA = getRigidBodyA().getInvMass();
btScalar miB = getRigidBodyB().getInvMass();
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
btVector3 ax1A = trA.getBasis().getColumn(2);
btVector3 ax1B = trB.getBasis().getColumn(2);
btVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
int s0 = 0;
int s1 = s;
int s2 = s * 2;
int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to hinge axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
btVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill three rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
btScalar k = info->fps * info->erp;
if (!m_angularOnly)
{
//.........这里部分代码省略.........
开发者ID:Rocket-Buddha,项目名称:GameCode4,代码行数:101,代码来源:btHingeConstraint.cpp
示例12: contactNormalOnB
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
int numDim = BTMBP2PCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
contactNormalOnB[i%3] = -1;
#endif
btScalar penetration = 0;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
}
btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
fillMultiBodyConstraint(constraintRow, data, 0, 0,
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
//@todo: support the case of btMultiBody versus btRigidBody,
//see btPoint2PointConstraint::getInfo2NonVirtual
#else
const btVector3 dummy(0, 0, 0);
btAssert(m_bodyA->isMultiDof());
btScalar* jac1 = jacobianA(i);
const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
dummy, dummy, dummy, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
#endif
}
}
开发者ID:DJoser,项目名称:gamekit,代码行数:88,代码来源:btMultiBodyPoint2Point.cpp
示例13: btAssert
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
{
btAssert((index >= 0) && (index < 6));
m_equilibriumPoint[index] = val;
}
开发者ID:0302zq,项目名称:libgdx,代码行数:5,代码来源:btGeneric6DofSpringConstraint.cpp
示例14: btAlignedFree
//.........这里部分代码省略.........
do
{
int src = edge->getSourceVertex();
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
btVector3 wa = convexUtil->vertices[src];
btVector3 wb = convexUtil->vertices[targ];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
btScalar planeEq = 1e30f;
if (numEdges==2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
tmpFaces[i].m_plane[0] = faceNormals[i].getX();
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
btAssert(0);//degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<tmpFaces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces and copy them to m_polyhedron
btScalar faceWeldThreshold= 0.999f;
btAlignedObjectArray<int> todoFaces;
for (int i=0;i<tmpFaces.size();i++)
todoFaces.push_back(i);
while (todoFaces.size())
{
btAlignedObjectArray<int> coplanarFaceGroup;
int refFace = todoFaces[todoFaces.size()-1];
coplanarFaceGroup.push_back(refFace);
btFace& faceA = tmpFaces[refFace];
todoFaces.pop_back();
btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
for (int j=todoFaces.size()-1;j>=0;j--)
开发者ID:dreamsxin,项目名称:nawia,代码行数:67,代码来源:btPolyhedralConvexShape.cpp
示例15: cullPoints2
void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
btScalar a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = btScalar(0.5)*(p[0] + p[2]);
cy = btScalar(0.5)*(p[1] + p[3]);
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
a += q;
cx += q*(p[i*2]+p[i*2+2]);
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
if (btFabs(a+q) > SIMD_EPSILON)
{
a = 1.f/(btScalar(3.0)*(a+q));
} else
{
a=BT_LARGE_FLOAT;
}
cx = a*(cx + q*(p[n*2-2]+p[0]));
cy = a*(cy + q*(p[n*2-1]+p[1]));
}
// compute the angle of each point w.r.t. the centroid
btScalar A[8];
for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
// search for points that have angles closest to A[i0] + i*(2*pi/m).
int avail[8];
for (i=0; i<n; i++) avail[i] = 1;
avail[i0] = 0;
iret[0] = i0;
iret++;
for (j=1; j<m; j++) {
a = btScalar(j)*(2*M__PI/m) + A[i0];
if (a > M__PI) a -= 2*M__PI;
btScalar maxdiff=1e9,diff;
*iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
for (i=0; i<n; i++) {
if (avail[i]) {
diff = btFabs (A[i]-a);
if (diff > M__PI) diff = 2*M__PI - diff;
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
#if defined(DEBUG) || defined (_DEBUG)
btAssert (*iret != i0); // ensure iret got set
#endif
avail[*iret] = 0;
iret++;
}
}
开发者ID:JinMyong,项目名称:Game_MatchEmUp,代码行数:69,代码来源:btBoxBoxDetector.cpp
示例16: finalizeMultiDof
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
if (rhs>m_rhsClamp)
{
rhs=m_rhsClamp;
}
if (rhs<-m_rhsClamp)
{
rhs=-m_rhsClamp;
}
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}
开发者ID:93i,项目名称:godot,代码行数:82,代码来源:btMultiBodyJointMotor.cpp
示例17: btAssert
void btHeightfieldTerrainShape::initialize
(
int heightStickWidth, int heightStickLength, const void* heightfieldData,
btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
PHY_ScalarType hdt, bool flipQuadEdges
)
{
// validation
btAssert(heightStickWidth > 1 && "bad width");
btAssert(heightStickLength > 1 && "bad length");
btAssert(heightfieldData && "null heightfield data");
// btAssert(heightScale) -- do we care? Trust caller here
btAssert(minHeight <= maxHeight && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3 &&
"bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
"Bad height data type enum");
// initialize member variables
m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
m_heightStickWidth = heightStickWidth;
m_heightStickLength = heightStickLength;
m_minHeight = minHeight;
m_maxHeight = maxHeight;
m_width = (btScalar) (heightStickWidth - 1);
m_length = (btScalar) (heightStickLength - 1);
m_heightScale = heightScale;
m_heightfieldDataUnknown = heightfieldData;
m_heightDataType = hdt;
m_flipQuadEdges = flipQuadEdges;
m_useDiamondSubdivision = false;
m_useZigzagSubdivision = false;
m_upAxis = upAxis;
m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
// determine min/max axis-aligned bounding box (aabb) values
switch (m_upAxis)
{
case 0:
{
m_localAabbMin.setValue(m_minHeight, 0, 0);
m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
break;
}
case 1:
{
m_localAabbMin.setValue(0, m_minHeight, 0);
m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
break;
};
case 2:
{
m_localAabbMin.setValue(0, 0, m_minHeight);
m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
break;
}
default:
{
//need to get valid m_upAxis
btAssert(0 && "Bad m_upAxis");
}
}
// remember origin (defined as exact middle of aabb)
m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
}
开发者ID:1005491398,项目名称:Threes,代码行数:66,代码来源:btHeightfieldTerrainShape.cpp
|
请发表评论