void StVKHessianTensor::AddTensor3x3x3Block(int v1, int v2, int v3, Vec3d & vec, int type) { #define TENSOR3_INDEX(i,j,k) (9*(i)+3*(j)+(k)) // seek for the Hessian term double * entry = NULL; hessianType::iterator pos; pos = hessian.find(triIndex(v1,v2,v3)); if (pos == hessian.end()) { // data is new entry = (double*) calloc (27,sizeof(double)); hessian.insert(std::make_pair(triIndex(v1,v2,v3),entry)); } else { // data already exists entry = pos->second; } // modify entry as necessary switch (type) { case 1: // vec tensor u for(int j=0; j<3; j++) for(int i=0; i<3; i++) entry[TENSOR3_INDEX(i,j,j)] += vec[i]; break; case 2: // u tensor vec for(int j=0; j<3; j++) for(int i=0; i<3; i++) entry[TENSOR3_INDEX(j,i,j)] += vec[i]; break; case 3: // <u,vec>I for(int j=0; j<3; j++) for(int i=0; i<3; i++) entry[TENSOR3_INDEX(j,j,i)] += vec[i]; break; default: printf("Error: type with tensor3 invalid.\n"); } }
void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { //just for debugging purposes //printf("triangle %d",m_triangleCount++); btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe) { btVector3 color(1,1,0); btTransform& tr = ob->getWorldTransform(); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); } btTriIndex triIndex(partId,triangleIndex,0); btHashKey<btTriIndex> triKey(triIndex.getUid()); btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { btCollisionShape* tm = shapeIndex->m_childShape; btAssert(tm); //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); ob->internalSetTemporaryCollisionShape( tmpShape); return; } //aabb filter is already applied! //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); // if (m_softBody->getCollisionShape()->getShapeType()== { // btVector3 other; btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); normal.normalize(); normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; // other+=normal*22.f; btVector3 pts[6] = {triangle[0]+normal, triangle[1]+normal, triangle[2]+normal, triangle[0]-normal, triangle[1]-normal, triangle[2]-normal}; btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); // tm.setMargin(m_collisionMarginTriangle); //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); //m_resultOut->setShapeIdentifiersB(partId,triangleIndex); // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); ob->internalSetTemporaryCollisionShape( tmpShape ); triIndex.m_childShape = tm; m_shapeCache.insert(triKey,triIndex); } }