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