void processConstraints()
{
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
}
virtual void btcMapPairBuffer(btcAabbSpace aabbSpace, int* numPairs, unsigned char** proxyAbase, unsigned char** proxyBbase,int* proxyType, int* pairStrideInBytes)
{
*numPairs = m_simpleBP->getOverlappingPairCache()->getNumOverlappingPairs();
if (numPairs>0)
{
m_simpleBP->getOverlappingPairCache()->getOverlappingPairArray()[0];
m_mappedPairs.resize(*numPairs*2);
for (int i=0;i<*numPairs;i++)
{
m_mappedPairs[i*2] = m_simpleBP->getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->m_clientObject;
m_mappedPairs[i*2+1] = m_simpleBP->getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->m_clientObject;
//printf("pair %d = (%p, %p)\n", *numPairs, m_simpleBP->getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->m_clientObject,
// m_simpleBP->getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->m_clientObject);
}
*proxyAbase = (unsigned char*)&(m_mappedPairs[0]);
*proxyBbase = (unsigned char*)&(m_mappedPairs[1]);
*proxyType = BTB_FLOAT_TYPE;
*pairStrideInBytes = sizeof(void*)*2;
}
}
SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_multiBodySortedConstraints = sortedMultiBodyConstraints;
m_numMultiBodyConstraints = numMultiBodyConstraints;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.resize (0);
m_manifolds.resize (0);
m_constraints.resize (0);
m_multiBodyConstraints.resize(0);
}
void processConstraints()
{
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
int main(int argc, char *argv[])
{
preInitGL(argc, argv);
#ifdef _WIN32
glewInit();
#endif
#ifdef USE_GPU_COPY
#ifdef _WIN32
HGLRC glCtx = wglGetCurrentContext();
#else //!_WIN32
GLXContext glCtx = glXGetCurrentContext();
#endif //!_WIN32
HDC glDC = wglGetCurrentDC();
initCL(glCtx, glDC);
#else
initCL();
#endif
cloths.resize(numFlags);
for( int flagIndex = 0; flagIndex < numFlags; ++flagIndex )
{
cloths[flagIndex].create_buffers(clothWidth, clothHeight);
}
initBullet();
m_dynamicsWorld->stepSimulation(1./60.,0);
std::string flagTexs[] = {
"bullet_logo.png",
"bullet_logo.png",
};
int numFlagTexs = 2;
for( int flagIndex = 0; flagIndex < numFlags; ++flagIndex )
{
cloths[flagIndex].create_texture(flagTexs[flagIndex % numFlagTexs]);
cloths[flagIndex].x_offset = 0;
cloths[flagIndex].y_offset = 0;
cloths[flagIndex].z_offset = 0;
}
goGL();
if( g_openCLSolver )
delete g_openCLSolver;
if( g_openCLSIMDSolver )
delete g_openCLSIMDSolver;
if( g_softBodyOutput )
delete g_softBodyOutput;
return 0;
}
/**
* Insert element into vectorToUpdate at index index.
*/
template< typename T > static void insertAtIndex( btAlignedObjectArray< T > &vectorToUpdate, int index, T element )
{
vectorToUpdate.resize( vectorToUpdate.size() + 1 );
for( int i = (vectorToUpdate.size() - 1); i > index; --i )
{
vectorToUpdate[i] = vectorToUpdate[i-1];
}
vectorToUpdate[index] = element;
}
// Function to remove an object from a vector maintaining correct ordering of the vector
template< typename T > static void removeFromVector( btAlignedObjectArray< T > &vectorToUpdate, int indexToRemove )
{
int currentSize = vectorToUpdate.size();
for( int i = indexToRemove; i < (currentSize-1); ++i )
{
vectorToUpdate[i] = vectorToUpdate[i+1];
}
if( currentSize > 0 )
vectorToUpdate.resize( currentSize - 1 );
}
void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
childLinkIndices.resize(0);
int numChildren = m_data->m_links[linkIndex]->child_links.size();
for (int i=0;i<numChildren;i++)
{
int childIndex =m_data->m_links[linkIndex]->child_links[i]->m_link_index;
childLinkIndices.push_back(childIndex);
}
}
// TODO: this routine appears to be numerically instable ...
void getVerticesInsidePlanes(const btAlignedObjectArray<btVector3>& planes,
btAlignedObjectArray<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);
}
}
}
}
}
}
}
}
void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
childLinkIndices.resize(0);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
//int numChildren = m_data->m_urdfParser->getModel().m_links.getAtIndex(linkIndex)->
for (int i=0;i<link->m_childLinks.size();i++)
{
int childIndex =link->m_childLinks[i]->m_linkIndex;
childLinkIndices.push_back(childIndex);
}
}
}
static void generateLinksPerVertex( int numVertices, btSoftBodyLinkData &linkData, btAlignedObjectArray< int > &listOfLinksPerVertex, btAlignedObjectArray <int> &numLinksPerVertex, int &maxLinks )
{
for( int linkIndex = 0; linkIndex < linkData.getNumLinks(); ++linkIndex )
{
btSoftBodyLinkData::LinkNodePair nodes( linkData.getVertexPair(linkIndex) );
numLinksPerVertex[nodes.vertex0]++;
numLinksPerVertex[nodes.vertex1]++;
}
int maxLinksPerVertex = 0;
for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
{
maxLinksPerVertex = btMax(numLinksPerVertex[vertexIndex], maxLinksPerVertex);
}
maxLinks = maxLinksPerVertex;
btAlignedObjectArray< int > linksFoundPerVertex;
linksFoundPerVertex.resize( numVertices, 0 );
listOfLinksPerVertex.resize( maxLinksPerVertex * numVertices );
for( int linkIndex = 0; linkIndex < linkData.getNumLinks(); ++linkIndex )
{
btSoftBodyLinkData::LinkNodePair nodes( linkData.getVertexPair(linkIndex) );
{
// Do vertex 0
int vertexIndex = nodes.vertex0;
int linkForVertex = linksFoundPerVertex[nodes.vertex0];
int linkAddress = vertexIndex * maxLinksPerVertex + linkForVertex;
listOfLinksPerVertex[linkAddress] = linkIndex;
linksFoundPerVertex[nodes.vertex0] = linkForVertex + 1;
}
{
// Do vertex 1
int vertexIndex = nodes.vertex1;
int linkForVertex = linksFoundPerVertex[nodes.vertex1];
int linkAddress = vertexIndex * maxLinksPerVertex + linkForVertex;
listOfLinksPerVertex[linkAddress] = linkIndex;
linksFoundPerVertex[nodes.vertex1] = linkForVertex + 1;
}
}
}
void updateTransforms()
{
int numObjects = m_shapePtr.size();
m_transforms.resize(numObjects);
for (int i=0;i<numObjects;i++)
{
m_transforms[i].setIdentity();
//btVector3 pos(0.f,-(2.5* numObjects * 0.5)+i*2.5f, 0.f);
btVector3 pos(0.f,+i*2.5f, 0.f);
m_transforms[i].setIdentity();
m_transforms[i].setOrigin( pos );
btQuaternion orn;
if (i < 2)
{
orn.setEuler(m_yaw,m_pitch,m_roll);
m_transforms[i].setRotation(orn);
}
}
if (m_animateRenderer)
{
m_pitch += 0.005f;
m_yaw += 0.01f;
}
}
static void computeBatchingIntoWavefronts(
btSoftBodyLinkData &linkData,
int wavefrontSize,
int linksPerWorkItem,
int maxLinksPerWavefront,
btAlignedObjectArray < btAlignedObjectArray <int> > &linksForWavefronts,
btAlignedObjectArray< btAlignedObjectArray < btAlignedObjectArray <int> > > &batchesWithinWaves, /* wave, batch, links in batch */
btAlignedObjectArray< btAlignedObjectArray< int > > &verticesForWavefronts /* wavefront, vertex */
)
{
// Attempt generation of larger batches of links.
btAlignedObjectArray< bool > processedLink;
processedLink.resize( linkData.getNumLinks() );
btAlignedObjectArray< int > listOfLinksPerVertex;
int maxLinksPerVertex = 0;
// Count num vertices
int numVertices = 0;
for( int linkIndex = 0; linkIndex < linkData.getNumLinks(); ++linkIndex )
{
btSoftBodyLinkData::LinkNodePair nodes( linkData.getVertexPair(linkIndex) );
numVertices = btMax( numVertices, nodes.vertex0 + 1 );
numVertices = btMax( numVertices, nodes.vertex1 + 1 );
}
// Need list of links per vertex
// Compute valence of each vertex
btAlignedObjectArray <int> numLinksPerVertex;
numLinksPerVertex.resize(0);
numLinksPerVertex.resize( numVertices, 0 );
generateLinksPerVertex( numVertices, linkData, listOfLinksPerVertex, numLinksPerVertex, maxLinksPerVertex );
if (!numVertices)
return;
for( int vertex = 0; vertex < 10; ++vertex )
{
for( int link = 0; link < numLinksPerVertex[vertex]; ++link )
{
int linkAddress = vertex * maxLinksPerVertex + link;
}
}
// At this point we know what links we have for each vertex so we can start batching
// We want a vertex to start with, let's go with 0
int currentVertex = 0;
int linksProcessed = 0;
btAlignedObjectArray <int> verticesToProcess;
while( linksProcessed < linkData.getNumLinks() )
{
// Next wavefront
int nextWavefront = linksForWavefronts.size();
linksForWavefronts.resize( nextWavefront + 1 );
btAlignedObjectArray <int> &linksForWavefront(linksForWavefronts[nextWavefront]);
verticesForWavefronts.resize( nextWavefront + 1 );
btAlignedObjectArray<int> &vertexSet( verticesForWavefronts[nextWavefront] );
linksForWavefront.resize(0);
// Loop to find enough links to fill the wavefront
// Stopping if we either run out of links, or fill it
while( linksProcessed < linkData.getNumLinks() && linksForWavefront.size() < maxLinksPerWavefront )
{
// Go through the links for the current vertex
for( int link = 0; link < numLinksPerVertex[currentVertex] && linksForWavefront.size() < maxLinksPerWavefront; ++link )
{
int linkAddress = currentVertex * maxLinksPerVertex + link;
int linkIndex = listOfLinksPerVertex[linkAddress];
// If we have not already processed this link, add it to the wavefront
// Claim it as another processed link
// Add the vertex at the far end to the list of vertices to process.
if( !processedLink[linkIndex] )
{
linksForWavefront.push_back( linkIndex );
linksProcessed++;
processedLink[linkIndex] = true;
int v0 = linkData.getVertexPair(linkIndex).vertex0;
int v1 = linkData.getVertexPair(linkIndex).vertex1;
if( v0 == currentVertex )
verticesToProcess.push_back( v1 );
else
verticesToProcess.push_back( v0 );
}
}
if( verticesToProcess.size() > 0 )
{
// Get the element on the front of the queue and remove it
currentVertex = verticesToProcess[0];
removeFromVector( verticesToProcess, 0 );
} else {
// If we've not yet processed all the links, find the first unprocessed one
// and select one of its vertices as the current vertex
if( linksProcessed < linkData.getNumLinks() )
{
int searchLink = 0;
while( processedLink[searchLink] )
searchLink++;
currentVertex = linkData.getVertexPair(searchLink).vertex0;
}
}
}
// We have either finished or filled a wavefront
for( int link = 0; link < linksForWavefront.size(); ++link )
{
int v0 = linkData.getVertexPair( linksForWavefront[link] ).vertex0;
int v1 = linkData.getVertexPair( linksForWavefront[link] ).vertex1;
insertUniqueAndOrderedIntoVector( vertexSet, v0 );
insertUniqueAndOrderedIntoVector( vertexSet, v1 );
}
// Iterate over links mapped to the wave and batch those
// We can run a batch on each cycle trivially
batchesWithinWaves.resize( batchesWithinWaves.size() + 1 );
btAlignedObjectArray < btAlignedObjectArray <int> > &batchesWithinWave( batchesWithinWaves[batchesWithinWaves.size()-1] );
for( int link = 0; link < linksForWavefront.size(); ++link )
{
int linkIndex = linksForWavefront[link];
btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( linkIndex );
int batch = 0;
bool placed = false;
while( batch < batchesWithinWave.size() && !placed )
{
bool foundSharedVertex = false;
if( batchesWithinWave[batch].size() >= wavefrontSize )
{
// If we have already filled this batch, move on to another
foundSharedVertex = true;
} else {
for( int link2 = 0; link2 < batchesWithinWave[batch].size(); ++link2 )
{
btSoftBodyLinkData::LinkNodePair vertices2 = linkData.getVertexPair( (batchesWithinWave[batch])[link2] );
if( vertices.vertex0 == vertices2.vertex0 ||
vertices.vertex1 == vertices2.vertex0 ||
vertices.vertex0 == vertices2.vertex1 ||
vertices.vertex1 == vertices2.vertex1 )
{
foundSharedVertex = true;
break;
}
}
}
if( !foundSharedVertex )
{
batchesWithinWave[batch].push_back( linkIndex );
placed = true;
} else {
++batch;
}
}
if( batch == batchesWithinWave.size() && !placed )
{
batchesWithinWave.resize( batch + 1 );
batchesWithinWave[batch].push_back( linkIndex );
}
}
}
}
PfxInt32 BulletSetupContactConstraints(PfxSetupContactConstraintsParam ¶m)
{
// PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
//if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
manifolds.resize(0);
for(PfxUInt32 i=0;i<numContactPairs;i++) {
PfxConstraintPair &pair = contactPairs[i];
// if(!sce::PhysicsEffects::pfxCheckSolver(pair)) {
// continue;
//}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxContactManifold &contact = offsetContactManifolds[iConstraint];
btPersistentManifold& manifold = manifolds.expand();
memset(&manifold,0xff,sizeof(btPersistentManifold));
manifold.m_body0 = &rbs[iA];
manifold.m_body1 = &rbs[iB];
manifold.m_cachedPoints = contact.getNumContacts();
if (!contact.getNumContacts())
continue;
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidBody &bodyA = offsetRigidBodies[iA];
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxRigidBody &bodyB = offsetRigidBodies[iB];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
contact.setInternalFlag(0);
PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
if(contact.getDuration() > 1) restitution = 0.0f;
PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
manifold.m_cachedPoints = contact.getNumContacts();
manifold.m_contactProcessingThreshold = 0.01f;//SCE_PFX_CONTACT_THRESHOLD_NORMAL;
manifold.m_contactBreakingThreshold = 0.01f;
for(int j=0;j<contact.getNumContacts();j++) {
PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 ptA = pfxReadVector3(cp.m_localPointA);
manifold.m_pointCache[j].m_localPointA.setValue(ptA.getX(),ptA.getY(),ptA.getZ());
PfxVector3 ptB = pfxReadVector3(cp.m_localPointB);
manifold.m_pointCache[j].m_localPointB.setValue(ptB.getX(),ptB.getY(),ptB.getZ());
manifold.m_pointCache[j].m_normalWorldOnB.setValue(
cp.m_constraintRow[0].m_normal[0],
cp.m_constraintRow[0].m_normal[1],
cp.m_constraintRow[0].m_normal[2]);
manifold.m_pointCache[j].m_distance1 = cp.m_distance1;
manifold.m_pointCache[j].m_combinedFriction = friction;
manifold.m_pointCache[j].m_combinedRestitution = restitution;
manifold.m_pointCache[j].m_appliedImpulse = cp.m_constraintRow[0].m_accumImpulse;
manifold.m_pointCache[j].m_lateralFrictionDir1.setValue(
cp.m_constraintRow[1].m_normal[0],
cp.m_constraintRow[1].m_normal[1],
cp.m_constraintRow[1].m_normal[2]);
manifold.m_pointCache[j].m_appliedImpulseLateral1 = cp.m_constraintRow[1].m_accumImpulse;
manifold.m_pointCache[j].m_lateralFrictionDir2.setValue(
cp.m_constraintRow[2].m_normal[0],
cp.m_constraintRow[2].m_normal[1],
cp.m_constraintRow[2].m_normal[2]);
manifold.m_pointCache[j].m_appliedImpulseLateral2 = cp.m_constraintRow[2].m_accumImpulse;
manifold.m_pointCache[j].m_lateralFrictionInitialized = true;
manifold.m_pointCache[j].m_lifeTime = cp.m_duration;
btTransform trA = manifold.m_body0->getWorldTransform();
btTransform trB = manifold.m_body1->getWorldTransform();
manifold.m_pointCache[j].m_positionWorldOnA = trA( manifold.m_pointCache[j].m_localPointA );
manifold.m_pointCache[j].m_positionWorldOnB = trB( manifold.m_pointCache[j].m_localPointB );
//btVector3 m_localPointA;
//btVector3 m_localPointB;
//btVector3 m_positionWorldOnB;
//m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
//btVector3 m_positionWorldOnA;
/*
pfxSetupContactConstraint(
cp.m_constraintRow[0],
cp.m_constraintRow[1],
cp.m_constraintRow[2],
cp.m_distance,
restitution,
friction,
pfxReadVector3(cp.m_constraintRow[0].m_normal),
pfxReadVector3(cp.m_localPointA),
pfxReadVector3(cp.m_localPointB),
stateA,
stateB,
solverBodyA,
solverBodyB,
param.separateBias,
param.timeStep
);
*/
}
contact.setCompositeFriction(friction);
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
//--------------------------------------------------------------------------------------
// Create any D3D11 resources that aren't dependant on the back buffer
//--------------------------------------------------------------------------------------
HRESULT CALLBACK OnD3D11CreateDevice( ID3D11Device* pd3dDevice, const DXGI_SURFACE_DESC* pBackBufferSurfaceDesc,
void* pUserContext )
{
g_pd3dDevice = pd3dDevice;
HRESULT hr;
ID3D11DeviceContext* pd3dImmediateContext = DXUTGetD3D11DeviceContext();
V_RETURN( g_DialogResourceManager.OnD3D11CreateDevice( pd3dDevice, pd3dImmediateContext ) );
V_RETURN( g_D3DSettingsDlg.OnD3D11CreateDevice( pd3dDevice ) );
g_pTxtHelper = new CDXUTTextHelper( pd3dDevice, pd3dImmediateContext, &g_DialogResourceManager, 15 );
D3DXVECTOR3 vCenter( 0.25767413f, -28.503521f, 111.00689f);
FLOAT fObjectRadius = 378.15607f;
D3DXMatrixTranslation( &g_mCenterMesh, -vCenter.x, -vCenter.y, -vCenter.z );
D3DXMATRIXA16 m;
D3DXMatrixRotationY( &m, D3DX_PI );
g_mCenterMesh *= m;
D3DXMatrixRotationX( &m, D3DX_PI / 2.0f );
g_mCenterMesh *= m;
// Compile the shaders to a model based on the feature level we acquired
ID3DBlob* pVertexShaderBuffer = NULL;
ID3DBlob* pGeometryShaderBuffer = NULL;
ID3DBlob* pPixelShaderBuffer = NULL;
switch( DXUTGetD3D11DeviceFeatureLevel() )
{
case D3D_FEATURE_LEVEL_11_0:
V_RETURN( CompileShaderFromFile( L"cloth_renderer_VS.hlsl", "VSMain", "vs_5_0" , &pVertexShaderBuffer ) );
V_RETURN( CompileShaderFromFile( L"cloth_renderer_PS.hlsl", "GSMain", "gs_5_0" , &pGeometryShaderBuffer ) );
V_RETURN( CompileShaderFromFile( L"cloth_renderer_PS.hlsl", "PSMain", "ps_5_0" , &pPixelShaderBuffer ) );
break;
}
// Create the shaders
V_RETURN( pd3dDevice->CreateVertexShader( pVertexShaderBuffer->GetBufferPointer(),
pVertexShaderBuffer->GetBufferSize(), NULL, &g_pVertexShader ) );
V_RETURN( pd3dDevice->CreateGeometryShader( pGeometryShaderBuffer->GetBufferPointer(),
pGeometryShaderBuffer->GetBufferSize(), NULL, &g_pGeometryShader ) );
V_RETURN( pd3dDevice->CreatePixelShader( pPixelShaderBuffer->GetBufferPointer(),
pPixelShaderBuffer->GetBufferSize(), NULL, &g_pPixelShader ) );
V_RETURN( pd3dDevice->CreateInputLayout( layout, ARRAYSIZE( layout ), pVertexShaderBuffer->GetBufferPointer(),
pVertexShaderBuffer->GetBufferSize(), &g_pVertexLayout11 ) );
SAFE_RELEASE( pVertexShaderBuffer );
SAFE_RELEASE( pPixelShaderBuffer );
SAFE_RELEASE( pGeometryShaderBuffer );
// Load the mesh
V_RETURN( g_Mesh11.Create( pd3dDevice, L"tiny\\tiny.sdkmesh", true ) );
// Create a sampler state
D3D11_SAMPLER_DESC SamDesc;
SamDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_LINEAR;
SamDesc.AddressU = D3D11_TEXTURE_ADDRESS_WRAP;
SamDesc.AddressV = D3D11_TEXTURE_ADDRESS_WRAP;
SamDesc.AddressW = D3D11_TEXTURE_ADDRESS_WRAP;
SamDesc.MipLODBias = 0.0f;
SamDesc.MaxAnisotropy = 1;
SamDesc.ComparisonFunc = D3D11_COMPARISON_ALWAYS;
SamDesc.BorderColor[0] = SamDesc.BorderColor[1] = SamDesc.BorderColor[2] = SamDesc.BorderColor[3] = 0;
SamDesc.MinLOD = 0;
SamDesc.MaxLOD = D3D11_FLOAT32_MAX;
V_RETURN( pd3dDevice->CreateSamplerState( &SamDesc, &g_pSamLinear ) );
// Setup constant buffers
D3D11_BUFFER_DESC Desc;
Desc.Usage = D3D11_USAGE_DYNAMIC;
Desc.BindFlags = D3D11_BIND_CONSTANT_BUFFER;
Desc.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE;
Desc.MiscFlags = 0;
Desc.ByteWidth = sizeof( CB_VS_PER_OBJECT );
V_RETURN( pd3dDevice->CreateBuffer( &Desc, NULL, &g_pcbVSPerObject ) );
Desc.ByteWidth = sizeof( CB_PS_PER_OBJECT );
V_RETURN( pd3dDevice->CreateBuffer( &Desc, NULL, &g_pcbPSPerObject ) );
Desc.ByteWidth = sizeof( CB_PS_PER_FRAME );
V_RETURN( pd3dDevice->CreateBuffer( &Desc, NULL, &g_pcbPSPerFrame ) );
// Setup the camera's view parameters
D3DXVECTOR3 vecEye( 30.0f, 30.0f, -80.0f );
D3DXVECTOR3 vecAt ( 10.0f, 20.0f, -0.0f );
g_Camera.SetViewParams( &vecEye, &vecAt );
cloths.resize(numFlags);
for( int flagIndex = 0; flagIndex < numFlags; ++flagIndex )
{
cloths[flagIndex].create_buffers(clothWidth, clothHeight);
}
initBullet();
std::wstring flagTexsName[] = {
L"atiFlag.bmp",
L"amdFlag.bmp",
};
int numFlagTexs = 2;
WCHAR flagTexs[2][MAX_PATH];
HRESULT res = DXUTFindDXSDKMediaFileCch(flagTexs[0],MAX_PATH, flagTexsName[0].c_str());
res = DXUTFindDXSDKMediaFileCch(flagTexs[1],MAX_PATH, flagTexsName[1].c_str());
for( int flagIndex = 0; flagIndex < numFlags; ++flagIndex )
{
cloths[flagIndex].create_texture(flagTexs[flagIndex % numFlagTexs]);
cloths[flagIndex].x_offset = 0;
cloths[flagIndex].y_offset = 0;
cloths[flagIndex].z_offset = 0;
}
my_capsule.create_buffers(50,40);
my_capsule.create_texture();
//Turn off backface culling
D3D11_RASTERIZER_DESC rsDesc;
ZeroMemory(&rsDesc,sizeof(D3D11_RASTERIZER_DESC) );
rsDesc.CullMode = D3D11_CULL_NONE;
rsDesc.FillMode = D3D11_FILL_SOLID;
hr = pd3dDevice->CreateRasterizerState(&rsDesc, &g_pRasterizerState);
rsDesc.FillMode = D3D11_FILL_WIREFRAME;
hr = pd3dDevice->CreateRasterizerState(&rsDesc, &g_pRasterizerStateWF);
SAFE_RELEASE(pd3dImmediateContext);
return S_OK;
}
static void generateBatchesOfWavefronts( btAlignedObjectArray < btAlignedObjectArray <int> > &linksForWavefronts, btSoftBodyLinkData &linkData, int numVertices, btAlignedObjectArray < btAlignedObjectArray <int> > &wavefrontBatches )
{
// A per-batch map of truth values stating whether a given vertex is in that batch
// This allows us to significantly optimize the batching
btAlignedObjectArray <btAlignedObjectArray<bool> > mapOfVerticesInBatches;
for( int waveIndex = 0; waveIndex < linksForWavefronts.size(); ++waveIndex )
{
btAlignedObjectArray <int> &wavefront( linksForWavefronts[waveIndex] );
int batch = 0;
bool placed = false;
while( batch < wavefrontBatches.size() && !placed )
{
// Test the current batch, see if this wave shares any vertex with the waves in the batch
bool foundSharedVertex = false;
for( int link = 0; link < wavefront.size(); ++link )
{
btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( wavefront[link] );
if( (mapOfVerticesInBatches[batch])[vertices.vertex0] || (mapOfVerticesInBatches[batch])[vertices.vertex1] )
{
foundSharedVertex = true;
}
}
if( !foundSharedVertex )
{
wavefrontBatches[batch].push_back( waveIndex );
// Insert vertices into this batch too
for( int link = 0; link < wavefront.size(); ++link )
{
btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( wavefront[link] );
(mapOfVerticesInBatches[batch])[vertices.vertex0] = true;
(mapOfVerticesInBatches[batch])[vertices.vertex1] = true;
}
placed = true;
}
batch++;
}
if( batch == wavefrontBatches.size() && !placed )
{
wavefrontBatches.resize( batch + 1 );
wavefrontBatches[batch].push_back( waveIndex );
// And resize map as well
mapOfVerticesInBatches.resize( batch + 1 );
// Resize maps with total number of vertices
mapOfVerticesInBatches[batch].resize( numVertices+1, false );
// Insert vertices into this batch too
for( int link = 0; link < wavefront.size(); ++link )
{
btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( wavefront[link] );
(mapOfVerticesInBatches[batch])[vertices.vertex0] = true;
(mapOfVerticesInBatches[batch])[vertices.vertex1] = true;
}
}
}
mapOfVerticesInBatches.clear();
}
void BulletConstraintSolver()
{
btPgsSolver pgs;
btContactSolverInfo info;
rbs.resize(0);
for (int i=0;i<numRigidBodies;i++)
{
btRigidBody& rb = rbs.expandNonInitializing();
rb.m_companionId=-1;
rb.m_angularFactor.setValue(1,1,1);
rb.m_anisotropicFriction.setValue(1,1,1);
rb.m_invMass = bodies[i].getMassInv();
rb.m_linearFactor.setValue(1,1,1);
btVector3 pos(states[i].getPosition().getX(),states[i].getPosition().getY(),states[i].getPosition().getZ());
rb.m_worldTransform.setIdentity();
btQuaternion orn(states[i].getOrientation().getX(),states[i].getOrientation().getY(),states[i].getOrientation().getZ(),states[i].getOrientation().getW());
rb.m_worldTransform.setRotation(orn);
rb.m_worldTransform.setOrigin(pos);
PfxMatrix3 ori(states[i].getOrientation());
rb.m_worldTransform.setRotation(orn);
PfxMatrix3 inertiaInvWorld = ori * bodies[i].getInertiaInv() * transpose(ori);
rb.m_invInertiaWorld.setIdentity();
if (rb.m_invMass)
{
for (int row=0;row<3;row++)
{
for (int col=0;col<3;col++)
{
rb.m_invInertiaWorld[col][row] = inertiaInvWorld.getElem(col,row);
}
}
} else
{
rb.m_invInertiaWorld = btMatrix3x3(0,0,0,0,0,0,0,0,0);
}
rb.m_linearVelocity.setValue(states[i].getLinearVelocity().getX(),states[i].getLinearVelocity().getY(),states[i].getLinearVelocity().getZ());
rb.m_angularVelocity.setValue(states[i].getAngularVelocity().getX(),states[i].getAngularVelocity().getY(),states[i].getAngularVelocity().getZ());
// printf("body added\n");
}
btAlignedObjectArray<btCollisionObject*> bodyPtrs;
bodyPtrs.resize(rbs.size());
for (int i=0;i<rbs.size();i++)
{
bodyPtrs[i] = &rbs[i];
}
unsigned int numCurrentPairs = numPairs[pairSwap];
PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
PfxSetupContactConstraintsParam param;
param.contactPairs = currentPairs;
param.numContactPairs = numCurrentPairs;
param.offsetContactManifolds = contacts;
param.offsetRigidStates = states;
param.offsetRigidBodies = bodies;
param.offsetSolverBodies = solverBodies;
param.numRigidBodies = numRigidBodies;
param.timeStep = timeStep;
param.separateBias = separateBias;
BulletSetupContactConstraints(param);
btAlignedObjectArray<btPersistentManifold*> manifoldPtrs;
manifoldPtrs.resize(manifolds.size());
for (int i=0;i<manifolds.size();i++)
{
manifoldPtrs[i] = &manifolds[i];
}
if (bodyPtrs.size() && manifoldPtrs.size())
{
pgs.solveGroup(&bodyPtrs[0],bodyPtrs.size(),&manifoldPtrs[0],manifoldPtrs.size(),0,0,info,0,0,0);
for (int i=0;i<numRigidBodies;i++)
{
btVector3 linvel = rbs[i].getLinearVelocity();
btVector3 angvel = rbs[i].getAngularVelocity();
states[i].setLinearVelocity(PfxVector3(linvel.getX(),linvel.getY(),linvel.getZ()));
states[i].setAngularVelocity(PfxVector3(angvel.getX(),angvel.getY(),angvel.getZ()));
}
}
BulletWriteWarmstartContactConstraints(param);
}
void btMultiBody::fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
// temporary space
int num_links = getNumLinks();
scratch_v.resize(2*num_links + 2);
scratch_m.resize(num_links + 1);
btVector3 * v_ptr = &scratch_v[0];
btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(num_links);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
const btVector3 p_minus_com_world = contact_point - base_pos;
rot_from_world[0] = btMatrix3x3(base_quat);
p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
n_local[0] = rot_from_world[0] * normal;
// omega coeffients first.
btVector3 omega_coeffs;
omega_coeffs = p_minus_com_world.cross(normal);
jac[0] = omega_coeffs[0];
jac[1] = omega_coeffs[1];
jac[2] = omega_coeffs[2];
// then v coefficients
jac[3] = normal[0];
jac[4] = normal[1];
jac[5] = normal[2];
// Set remaining jac values to zero for now.
for (int i = 6; i < 6 + num_links; ++i) {
jac[i] = 0;
}
// Qdot coefficients, if necessary.
if (num_links > 0 && link > -1) {
// TODO: speed this up -- don't calculate for links we don't need.
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
// which is resulting in repeated work being done...)
// calculate required normals & positions in the local frames.
for (int i = 0; i < num_links; ++i) {
// transform to local frame
const int parent = links[i].parent;
const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
n_local[i+1] = mtx * n_local[parent+1];
p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;
// calculate the jacobian entry
if (links[i].is_revolute) {
results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
} else {
results[i] = n_local[i+1].dot( links[i].axis_bottom );
}
}
// Now copy through to output.
while (link != -1) {
jac[6 + link] = results[link];
link = links[link].parent;
}
}
}
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
{
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
int num_links = getNumLinks();
scratch_r.resize(num_links);
scratch_v.resize(4*num_links + 4);
btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
btVector3 * v_ptr = &scratch_v[0];
// zhat_i^A (scratch space)
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// rot_from_parent (cached from calcAccelerations)
const btMatrix3x3 * rot_from_parent = &matrix_buf[0];
// hhat (cached), accel (scratch)
const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i (scratch), D_i (cached)
btScalar * Y = r_ptr; r_ptr += num_links;
const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
btVector3 input_force(force[3],force[4],force[5]);
btVector3 input_torque(force[0],force[1],force[2]);
// Fill in zero_acc
// -- set to force/torque on the base, zero otherwise
if (fixed_base)
{
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else
{
zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
}
for (int i = 0; i < num_links; ++i)
{
zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
}
// 'Downward' loop.
for (int i = num_links - 1; i >= 0; --i)
{
Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
Y[i] += force[6 + i]; // add joint torque
const int parent = links[i].parent;
// Zp += pXi * (Zi + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] / D[i];
in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Second 'upward' loop
if (fixed_base)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
} else
{
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top,rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the links
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom;
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out;
omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out;
vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
}
void btMultiBody::stepVelocities(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m)
{
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations.
// We apply damping forces in this routine as well as any external forces specified by the
// caller (via addBaseForce etc).
// output should point to an array of 6 + num_links reals.
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
// num_links joint acceleration values.
int num_links = getNumLinks();
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
scratch_r.resize(2*num_links + 6);
scratch_v.resize(8*num_links + 6);
scratch_m.resize(4*num_links + 4);
btScalar * r_ptr = &scratch_r[0];
btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
btVector3 * v_ptr = &scratch_v[0];
// vhat_i (top = angular, bottom = linear part)
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
// zhat_i^A
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// chat_i (note NOT defined for the base)
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
// top left, top right and bottom left blocks of Ihat_i^A.
// bottom right block = transpose of top left block and is not stored.
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
// Cached 3x3 rotation matrices from parent frame to this frame.
btMatrix3x3 * rot_from_parent = &matrix_buf[0];
btMatrix3x3 * rot_from_world = &scratch_m[0];
// hhat_i, ahat_i
// hhat is NOT stored for the base (but ahat is)
btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i, D_i
btScalar * Y = r_ptr; r_ptr += num_links;
btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Start of the algorithm proper.
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
rot_from_parent[0] = btMatrix3x3(base_quat);
vel_top_angular[0] = rot_from_parent[0] * base_omega;
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
if (fixed_base) {
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else {
zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force
- base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
zero_acc_bottom_linear[0] =
- (rot_from_parent[0] * base_torque);
if (m_useGyroTerm)
zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );
zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
}
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[0].setValue(base_mass, 0, 0,
0, base_mass, 0,
0, 0, base_mass);
inertia_bottom_left[0].setValue(base_inertia[0], 0, 0,
0, base_inertia[1], 0,
0, 0, base_inertia[2]);
rot_from_world[0] = rot_from_parent[0];
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this);
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
// vhat_i = i_xhat_p(i) * vhat_p(i)
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
vel_top_angular[i+1], vel_bottom_linear[i+1]);
// we can now calculate chat_i
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector))
+ 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i);
if (links[i].is_revolute) {
coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i);
coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
} else {
coriolis_top_angular[i] = btVector3(0,0,0);
}
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;
// calculate zhat_i^A
zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
zero_acc_bottom_linear[i+1] =
- (rot_from_world[i+1] * links[i].applied_torque);
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(links[i].mass, 0, 0,
0, links[i].mass, 0,
0, 0, links[i].mass);
inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
0, links[i].inertia[1], 0,
0, 0, links[i].inertia[2]);
}
// 'Downward' loop.
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i) {
h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
D[i] = val;
Y[i] = links[i].joint_torque
- SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = links[i].parent;
// Ip += pXi * (Ii - hi hi' / Di) * iXp
const btScalar one_over_di = 1.0f / D[i];
const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
btMatrix3x3 r_cross;
r_cross.setValue(
0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
-links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0);
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] * one_over_di;
in_top = zero_acc_top_angular[i+1]
+ inertia_top_left[i+1] * coriolis_top_angular[i]
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
+ Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1]
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+ Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// Second 'upward' loop
// (part of TreeForwardDynamics in Mirtich)
if (fixed_base)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
}
else
{
if (num_links > 0)
{
//Matrix<btScalar, 6, 6> Imatrix;
//Imatrix.block<3,3>(0,0) = inertia_top_left[0];
//Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
//Imatrix.block<3,3>(0,3) = inertia_top_right[0];
//Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
//cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
cached_inertia_top_left = inertia_top_left[0];
cached_inertia_top_right = inertia_top_right[0];
cached_inertia_lower_left = inertia_bottom_left[0];
cached_inertia_lower_right= inertia_top_left[0].transpose();
}
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top, rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the links
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom;
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
// Final step: add the accelerations (times dt) to the velocities.
applyDeltaVee(output, dt);
}
void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintArray.size();
int n = numConstraintRows;
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
btScalar rhs = m_allConstraintArray[i].m_rhs;
btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration;
m_b[i]=rhs/jacDiag;
m_bSplit[i] = rhsPenetration/jacDiag;
}
}
}
btScalar* w = 0;
int nub = 0;
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
{
BT_PROFILE("init lo/ho");
for (int i=0;i<numConstraintRows;i++)
{
if (0)//m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
} else
{
m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
m_hi[i] = m_allConstraintArray[i].m_upperLimit;
}
}
}
//
int m=m_allConstraintArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies,-1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2*m_allConstraintArray.size());
}
static btMatrixXu J3;
{
BT_PROFILE("J3.resize");
J3.resize(2*m,8);
}
static btMatrixXu JinvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2*m,8);
JinvM3.setZero();
J3.setZero();
}
int cur=0;
int rowOffset = 0;
static btAlignedObjectArray<int> ofs;
{
BT_PROFILE("ofs resize");
ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintArray.size());
}
{
BT_PROFILE("Compute J and JinvM");
int c=0;
int numRows = 0;
for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintArray[i].m_solverBodyIdA;
int sbB = m_allConstraintArray[i].m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
if (orgBodyA)
{
{
int slotA=-1;
//find free jointNode slot for sbA
slotA =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
jointNodeArray[slotA].jointIndex = c;
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row=0;row<numRows;row++,cur++)
{
btVector3 normalInvMass = m_allConstraintArray[i+row].m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r=0;r<3;r++)
{
J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]);
J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMass[r]);
JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
}
J3.setElem(cur,3,0);
JinvM3.setElem(cur,3,0);
J3.setElem(cur,7,0);
JinvM3.setElem(cur,7,0);
}
} else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB=-1;
//find free jointNode slot for sbA
slotB =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
jointNodeArray[slotB].jointIndex = c;
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row=0;row<numRows;row++,cur++)
{
btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r=0;r<3;r++)
{
J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]);
J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]);
JinvM3.setElem(cur,r,normalInvMassB[r]);
JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
}
J3.setElem(cur,3,0);
JinvM3.setElem(cur,3,0);
J3.setElem(cur,7,0);
JinvM3.setElem(cur,7,0);
}
}
else
{
cur += numRows;
}
rowOffset+=numRows;
}
}
//compute JinvM = J*invM.
const btScalar* JinvM = JinvM3.getBufferPointer();
const btScalar* Jptr = J3.getBufferPointer();
{
BT_PROFILE("m_A.resize");
m_A.resize(n,n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c=0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
{
int row__ = ofs[c];
int sbA = m_allConstraintArray[i].m_solverBodyIdA;
int sbB = m_allConstraintArray[i].m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
{
int startJointNodeA = bodyJointNodeArray[sbA];
while (startJointNodeA>=0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0<c)
{
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r ( JinvMrow,
Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
}
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB>=0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1<c)
{
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther : 0;
m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows,
Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
}
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
}
}
}
{
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int numJointRows = m_allConstraintArray.size();
int jj=0;
for (;row__<numJointRows;)
{
int sbA = m_allConstraintArray[row__].m_solverBodyIdA;
int sbB = m_allConstraintArray[row__].m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
if (orgBodyB)
{
m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__);
}
row__ += infom;
jj++;
}
}
}
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
}
}
///fill the upper triangle of the matrix, to make it symmetric
{
BT_PROFILE("fill the upper triangle ");
m_A.copyLowerToUpperTriangle();
}
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
{
for (int i=0;i<m_allConstraintArray.size();i++)
{
const btSolverConstraint& c = m_allConstraintArray[i];
m_x[i]=c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
} else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void InvertedPendulumPDControl::stepSimulation(float deltaTime)
{
static btScalar offset = -0.1*SIMD_PI;
m_frameCount++;
if ((m_frameCount&0xff)==0 )
{
offset = -offset;
}
btScalar target= SIMD_PI+offset;
qDesiredArray.resize(0);
qDesiredArray.resize(m_multiBody->getNumLinks(),target);
for (int joint = 0; joint<m_multiBody->getNumLinks(); joint++)
{
int dof1 = 0;
btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1];
btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1];
btScalar positionError = (qDesiredArray[joint]-qActual);
double desiredVelocity = 0;
btScalar velocityError = (desiredVelocity-qdActual);
btScalar force = kp * positionError + kd*velocityError;
btClamp(force,-maxForce,maxForce);
m_multiBody->addJointTorque(joint, force);
}
if (m_frameCount==100)
{
const char* gPngFileName = "pendulum";
if (gPngFileName)
{
//printf("gPngFileName=%s\n",gPngFileName);
sprintf(fileName,"%s%d.png",gPngFileName,m_frameCount);
b3Printf("Made screenshot %s",fileName);
this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName);
}
}
m_dynamicsWorld->stepSimulation(1./60.,0);//240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
#if 0
for (int i=0; i<m_jointFeedbacks.size(); i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
#endif
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}