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 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 readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{
const char* nodeName = node->Attribute("id");
//printf("processing node %s\n", nodeName);
btMatrix4x4 nodeTrans;
nodeTrans.setIdentity();
///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
{
for (TiXmlElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
{
if (transElem->GetText())
{
btAlignedObjectArray<float> floatArray;
TokenFloatArray adder(floatArray);
tokenize(transElem->GetText(),adder);
if (floatArray.size()==16)
{
btMatrix4x4 t(floatArray[0],floatArray[1],floatArray[2],floatArray[3],
floatArray[4],floatArray[5],floatArray[6],floatArray[7],
floatArray[8],floatArray[9],floatArray[10],floatArray[11],
floatArray[12],floatArray[13],floatArray[14],floatArray[15]);
nodeTrans = nodeTrans*t;
} else
{
b3Warning("Error: expected 16 elements in a <matrix> element, skipping\n");
}
}
}
}
{
for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
{
if (transElem->GetText())
{
btVector3 pos = getVector3FromXmlText(transElem->GetText());
//nodePos+= unitScaling*parentScaling*pos;
btMatrix4x4 t;
t.setPureTranslation(pos);
nodeTrans = nodeTrans*t;
}
}
}
{
for(TiXmlElement* scaleElem = node->FirstChildElement("scale");
scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale"))
{
if (scaleElem->GetText())
{
btVector3 scaling = getVector3FromXmlText(scaleElem->GetText());
btMatrix4x4 t;
t.setPureScaling(scaling);
nodeTrans = nodeTrans*t;
}
}
}
{
for(TiXmlElement* rotateElem = node->FirstChildElement("rotate");
rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate"))
{
if (rotateElem->GetText())
{
//accumulate orientation
btVector4 rotate = getVector4FromXmlText(rotateElem->GetText());
btQuaternion orn(btVector3(rotate),btRadians(rotate[3]));//COLLADA DAE rotate is in degrees, convert to radians
btMatrix4x4 t;
t.setPureRotation(orn);
nodeTrans = nodeTrans*t;
}
}
}
nodeTrans = parentTransMat*nodeTrans;
for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry");
instanceGeom!=0;
instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
{
const char* geomUrl = instanceGeom->Attribute("url");
//printf("node referring to geom %s\n", geomUrl);
geomUrl++;
int* shapeIndexPtr = name2Shape[geomUrl];
if (shapeIndexPtr)
{
// int index = *shapeIndexPtr;
//printf("found geom with index %d\n", *shapeIndexPtr);
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = *shapeIndexPtr;
instance.m_worldTransform = nodeTrans;
} else
{
b3Warning("geom not found\n");
}
}
for(TiXmlElement* childNode = node->FirstChildElement("node");
childNode!= NULL; childNode = childNode->NextSiblingElement("node"))
{
readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans);
}
}
//--------------------------------------------------------------------------------------
// 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 inline T average(const btAlignedObjectArray<T>& items)
{
const btScalar n=(btScalar)(items.size()>0?items.size():1);
return(sum(items)/n);
}
//--------------------------------------------------------------------------------------
// Render the scene using the D3D11 device
//--------------------------------------------------------------------------------------
void CALLBACK OnD3D11FrameRender( ID3D11Device* pd3dDevice, ID3D11DeviceContext* pd3dImmediateContext, double fTime,
float fElapsedTime, void* pUserContext )
{
//float ms = getDeltaTimeMicroseconds();
btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
m_clock.reset();
///step the simulation
if (m_dynamicsWorld && !paused)
{
m_dynamicsWorld->stepSimulation(dt / 1000000.f);
updatePhysicsWorld();
}
//paused = 1;
///////////////////////////////////////////////////////
HRESULT hr;
// If the settings dialog is being shown, then render it instead of rendering the app's scene
if( g_D3DSettingsDlg.IsActive() )
{
g_D3DSettingsDlg.OnRender( fElapsedTime );
return;
}
// Clear the render target and depth stencil
float ClearColor[4] = { 0.0f, 0.25f, 0.25f, 0.55f };
ID3D11RenderTargetView* pRTV = DXUTGetD3D11RenderTargetView();
pd3dImmediateContext->ClearRenderTargetView( pRTV, ClearColor );
ID3D11DepthStencilView* pDSV = DXUTGetD3D11DepthStencilView();
pd3dImmediateContext->ClearDepthStencilView( pDSV, D3D11_CLEAR_DEPTH, 1.0, 0 );
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
cloths[flagIndex].draw();
}
my_capsule.draw();
DXUT_BeginPerfEvent( DXUT_PERFEVENTCOLOR, L"HUD / Stats" );
g_HUD.OnRender( fElapsedTime );
g_SampleUI.OnRender( fElapsedTime );
RenderText();
DXUT_EndPerfEvent();
/*
SAFE_RELEASE(pRTV);
SAFE_RELEASE(pDSV);
*/
}
/**
* Create a sequence of flag objects and add them to the world.
*/
void createFlag( int width, int height, btAlignedObjectArray<btSoftBody *> &flags )
{
// First create a triangle mesh to represent a flag
using namespace BTAcceleratedSoftBody;
using Vectormath::Aos::Matrix3;
using Vectormath::Aos::Vector3;
// Allocate a simple mesh consisting of a vertex array and a triangle index array
btIndexedMesh mesh;
mesh.m_numVertices = width*height;
mesh.m_numTriangles = 2*(width-1)*(height-1);
btVector3 *vertexArray = new btVector3[mesh.m_numVertices];
mesh.m_vertexBase = reinterpret_cast<const unsigned char*>(vertexArray);
int *triangleVertexIndexArray = new int[3*mesh.m_numTriangles];
mesh.m_triangleIndexBase = reinterpret_cast<const unsigned char*>(triangleVertexIndexArray);
mesh.m_triangleIndexStride = sizeof(int)*3;
mesh.m_vertexStride = sizeof(Vector3);
// Generate normalised object space vertex coordinates for a rectangular flag
float zCoordinate = 0.0f;
Matrix3 defaultScale(Vector3(5.f, 0.f, 0.f), Vector3(0.f, 20.f, 0.f), Vector3(0.f, 0.f, 1.f));
for( int y = 0; y < height; ++y )
{
float yCoordinate = y*2.0f/float(height) - 1.0f;
for( int x = 0; x < width; ++x )
{
float xCoordinate = x*2.0f/float(width) - 1.0f;
Vector3 vertex(xCoordinate, yCoordinate, zCoordinate);
Vector3 transformedVertex = defaultScale*vertex;
vertexArray[y*width + x] = btVector3(transformedVertex.getX(), transformedVertex.getY(), transformedVertex.getZ() );
}
}
// Generate vertex indices for triangles
for( int y = 0; y < (height-1); ++y )
{
for( int x = 0; x < (width-1); ++x )
{
// Triangle 0
// Top left of square on mesh
{
int vertex0 = y*width + x;
int vertex1 = vertex0 + 1;
int vertex2 = vertex0 + width;
int triangleIndex = 2*y*(width-1) + 2*x;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)] = vertex0;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex+1)/sizeof(int)+1] = vertex1;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex+2)/sizeof(int)+2] = vertex2;
}
// Triangle 1
// Bottom right of square on mesh
{
int vertex0 = y*width + x + 1;
int vertex1 = vertex0 + width;
int vertex2 = vertex1 - 1;
int triangleIndex = 2*y*(width-1) + 2*x + 1;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)] = vertex0;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)+1] = vertex1;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)+2] = vertex2;
}
}
}
float rotateAngleRoundZ = 0.5;
float rotateAngleRoundX = 0.5;
btMatrix3x3 defaultRotate;
defaultRotate[0] = btVector3(cos(rotateAngleRoundZ), sin(rotateAngleRoundZ), 0.f);
defaultRotate[1] = btVector3(-sin(rotateAngleRoundZ), cos(rotateAngleRoundZ), 0.f);
defaultRotate[2] = btVector3(0.f, 0.f, 1.f);
//btMatrix3x3 defaultRotateAndScale( (defaultRotateX*defaultRotate) );
#ifdef TABLETEST
btMatrix3x3 defaultRotateX;
rotateAngleRoundX = 3.141592654/2;
defaultRotateX[0] = btVector3(1.f, 0.f, 0.f);
defaultRotateX[1] = btVector3( 0.f, cos(rotateAngleRoundX), sin(rotateAngleRoundX));
defaultRotateX[2] = btVector3(0.f, -sin(rotateAngleRoundX), cos(rotateAngleRoundX));
btMatrix3x3 defaultRotateAndScale( (defaultRotateX) );
#else
btMatrix3x3 defaultRotateX;
defaultRotateX[0] = btVector3(1.f, 0.f, 0.f);
defaultRotateX[1] = btVector3( 0.f, cos(rotateAngleRoundX), sin(rotateAngleRoundX));
defaultRotateX[2] = btVector3(0.f, -sin(rotateAngleRoundX), cos(rotateAngleRoundX));
btMatrix3x3 defaultRotateAndScale( (defaultRotateX) );
#endif
// Construct the sequence flags applying a slightly different translation to each one to arrange them
// appropriately in the scene.
for( int i = 0; i < numFlags; ++i )
{
float zTranslate = flagSpacing * (i-numFlags/2);
btVector3 defaultTranslate(0.f, 20.f, zTranslate);
btTransform transform( defaultRotateAndScale, defaultTranslate );
btSoftBody *softBody = createFromIndexedMesh( vertexArray, mesh.m_numVertices, triangleVertexIndexArray, mesh.m_numTriangles, true );
for( int i = 0; i < mesh.m_numVertices; ++i )
{
softBody->setMass(i, 10.f/mesh.m_numVertices);
}
#ifndef TABLETEST
// Set the fixed points
softBody->setMass((height-1)*(width), 0.f);
softBody->setMass((height-1)*(width) + width - 1, 0.f);
softBody->setMass((height-1)*width + width/2, 0.f);
#endif
softBody->m_cfg.collisions = btSoftBody::fCollision::CL_SS+btSoftBody::fCollision::CL_RS;
softBody->m_cfg.kLF = 0.0005f;
softBody->m_cfg.kVCF = 0.001f;
softBody->m_cfg.kDP = 0.f;
softBody->m_cfg.kDG = 0.f;
flags.push_back( softBody );
softBody->transform( transform );
softBody->setFriction( 0.8f );
m_dynamicsWorld->addSoftBody( softBody );
}
delete [] vertexArray;
delete [] triangleVertexIndexArray;
}
void PhysicsClient::submitCommand(int command)
{
m_userCommandRequests.push_back(command);
b3Printf("User submitted command request %d (outstanding %d)\n",command, m_userCommandRequests.size());
}
void VoronoiFractureDemo::voronoiConvexHullShatter(const btAlignedObjectArray<btVector3>& points, const btAlignedObjectArray<btVector3>& verts, const btQuaternion& bbq, const btVector3& bbt, btScalar matDensity) {
// points define voronoi cells in world space (avoid duplicates)
// verts = source (convex hull) mesh vertices in local space
// bbq & bbt = source (convex hull) mesh quaternion rotation and translation
// matDensity = Material density for voronoi shard mass calculation
btConvexHullComputer* convexHC = new btConvexHullComputer();
btAlignedObjectArray<btVector3> vertices, chverts;
btVector3 rbb, nrbb;
btScalar nlength, maxDistance, distance;
btAlignedObjectArray<btVector3> sortedVoronoiPoints;
sortedVoronoiPoints.copyFromArray(points);
btVector3 normal, plane;
btAlignedObjectArray<btVector3> planes, convexPlanes;
std::set<int> planeIndices;
std::set<int>::iterator planeIndicesIter;
int numplaneIndices;
int cellnum = 0;
int i, j, k;
// Convert verts to world space and get convexPlanes
int numverts = verts.size();
chverts.resize(verts.size());
for (i=0; i < numverts ;i++) {
chverts[i] = quatRotate(bbq, verts[i]) + bbt;
}
//btGeometryUtil::getPlaneEquationsFromVertices(chverts, convexPlanes);
// Using convexHullComputer faster than getPlaneEquationsFromVertices for large meshes...
convexHC->compute(&chverts[0].getX(), sizeof(btVector3), numverts, 0.0, 0.0);
int numFaces = convexHC->faces.size();
int v0, v1, v2; // vertices
for (i=0; i < numFaces; i++) {
const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[i]];
v0 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
edge = edge->getNextEdgeOfFace();
v2 = edge->getTargetVertex();
plane = (convexHC->vertices[v1]-convexHC->vertices[v0]).cross(convexHC->vertices[v2]-convexHC->vertices[v0]).normalize();
plane[3] = -plane.dot(convexHC->vertices[v0]);
convexPlanes.push_back(plane);
}
const int numconvexPlanes = convexPlanes.size();
int numpoints = points.size();
for (i=0; i < numpoints ;i++) {
curVoronoiPoint = points[i];
planes.copyFromArray(convexPlanes);
for (j=0; j < numconvexPlanes ;j++) {
planes[j][3] += planes[j].dot(curVoronoiPoint);
}
maxDistance = SIMD_INFINITY;
sortedVoronoiPoints.heapSort(pointCmp());
for (j=1; j < numpoints; j++) {
normal = sortedVoronoiPoints[j] - curVoronoiPoint;
nlength = normal.length();
if (nlength > maxDistance)
break;
plane = normal.normalized();
plane[3] = -nlength / btScalar(2.);
planes.push_back(plane);
getVerticesInsidePlanes(planes, vertices, planeIndices);
if (vertices.size() == 0)
break;
numplaneIndices = planeIndices.size();
if (numplaneIndices != planes.size()) {
planeIndicesIter = planeIndices.begin();
for (k=0; k < numplaneIndices; k++) {
if (k != *planeIndicesIter)
planes[k] = planes[*planeIndicesIter];
planeIndicesIter++;
}
planes.resize(numplaneIndices);
}
maxDistance = vertices[0].length();
for (k=1; k < vertices.size(); k++) {
distance = vertices[k].length();
if (maxDistance < distance)
maxDistance = distance;
}
maxDistance *= btScalar(2.);
}
if (vertices.size() == 0)
continue;
// Clean-up voronoi convex shard vertices and generate edges & faces
convexHC->compute(&vertices[0].getX(), sizeof(btVector3), vertices.size(),0.0,0.0);
// At this point we have a complete 3D voronoi shard mesh contained in convexHC
// Calculate volume and center of mass (Stan Melax volume integration)
numFaces = convexHC->faces.size();
btScalar volume = btScalar(0.);
btVector3 com(0., 0., 0.);
for (j=0; j < numFaces; j++) {
const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[j]];
v0 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
edge = edge->getNextEdgeOfFace();
v2 = edge->getTargetVertex();
while (v2 != v0) {
// Counter-clockwise triangulated voronoi shard mesh faces (v0-v1-v2) and edges here...
btScalar vol = convexHC->vertices[v0].triple(convexHC->vertices[v1], convexHC->vertices[v2]);
volume += vol;
com += vol * (convexHC->vertices[v0] + convexHC->vertices[v1] + convexHC->vertices[v2]);
edge = edge->getNextEdgeOfFace();
v1 = v2;
v2 = edge->getTargetVertex();
}
}
com /= volume * btScalar(4.);
volume /= btScalar(6.);
// Shift all vertices relative to center of mass
int numVerts = convexHC->vertices.size();
for (j=0; j < numVerts; j++)
{
convexHC->vertices[j] -= com;
}
// Note:
// At this point convex hulls contained in convexHC should be accurate (line up flush with other pieces, no cracks),
// ...however Bullet Physics rigid bodies demo visualizations appear to produce some visible cracks.
// Use the mesh in convexHC for visual display or to perform boolean operations with.
// Create Bullet Physics rigid body shards
btCollisionShape* shardShape = new btConvexHullShape(&(convexHC->vertices[0].getX()), convexHC->vertices.size());
shardShape->setMargin(CONVEX_MARGIN); // for this demo; note convexHC has optional margin parameter for this
m_collisionShapes.push_back(shardShape);
btTransform shardTransform;
shardTransform.setIdentity();
shardTransform.setOrigin(curVoronoiPoint + com); // Shard's adjusted location
btDefaultMotionState* shardMotionState = new btDefaultMotionState(shardTransform);
btScalar shardMass(volume * matDensity);
btVector3 shardInertia(0.,0.,0.);
shardShape->calculateLocalInertia(shardMass, shardInertia);
btRigidBody::btRigidBodyConstructionInfo shardRBInfo(shardMass, shardMotionState, shardShape, shardInertia);
btRigidBody* shardBody = new btRigidBody(shardRBInfo);
m_dynamicsWorld->addRigidBody(shardBody);
cellnum ++;
}
printf("Generated %d voronoi btRigidBody shards\n", cellnum);
}
void PhysicsClient::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
{
bool isTrigger = false;
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
} else
{
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
//m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_testBlock1)
{
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
b3Error("Error: please start server before client\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0;
} else
{
b3Printf("Shared Memory status is OK\n");
}
} else
{
m_wantsTermination = true;
}
}
void PhysicsClient::createClientCommand()
{
if (!m_waitingForServer)
{
//process outstanding requests
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int command = m_userCommandRequests[0];
//don't use 'remove' because it will re-order the commands
//m_userCommandRequests.remove(command);
//pop_front
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
m_waitingForServer = true;
switch (command)
{
case CMD_LOAD_URDF:
{
if (!m_serverLoadUrdfOK)
{
m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF\n");
} else
{
b3Warning("Server already loaded URDF, no client command submitted\n");
}
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_serverLoadUrdfOK)
{
b3Printf("Requesting actual state\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("No URDF loaded\n");
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
if (m_serverLoadUrdfOK)
{
m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
} else
{
b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
}
break;
}
case CMD_SHUTDOWN:
{
m_wantsTermination = true;
m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_testBlock1->m_numClientCommands++;
m_serverLoadUrdfOK = false;
b3Printf("client created CMD_SHUTDOWN\n");
break;
}
default:
{
b3Error("unknown command requested\n");
}
}
}
}
}
///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands( btDispatcher* dispatcher,
btCollisionWorld* collisionWorld,
btAlignedObjectArray<btTypedConstraint*>& constraints,
IslandCallback* callback
)
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
buildIslands(dispatcher,collisionWorld);
BT_PROFILE("processIslands");
if(!m_splitIslands)
{
btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds();
for ( int i = 0; i < maxNumManifolds; i++ )
{
btPersistentManifold* manifold = manifolds[ i ];
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );
///@todo: check sleeping conditions!
if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
{
//kinematic objects don't merge islands, but wake up all connected objects
if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
{
if ( colObj0->hasContactResponse() )
colObj1->activate();
}
if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
{
if ( colObj1->hasContactResponse() )
colObj0->activate();
}
}
}
btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
callback->processIsland(&collisionObjects[0],
collisionObjects.size(),
manifolds,
maxNumManifolds,
constraintsPtr,
constraints.size(),
-1
);
}
else
{
initIslandPools();
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
addBodiesToIslands( collisionWorld );
addManifoldsToIslands( dispatcher );
addConstraintsToIslands( constraints );
// m_activeIslands array should now contain all non-sleeping Islands, and each Island should
// have all the necessary bodies, manifolds and constraints.
// if we want to merge islands with small batch counts,
if ( m_minimumSolverBatchSize > 1 )
{
mergeIslands();
}
// dispatch islands to solver
m_islandDispatch( &m_activeIslands, callback );
}
}
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];
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
//calc centroid, to shift vertices around center of mass
btVector3 centroid(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if(m_transformSubShapes)
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
centroid += vertex;
}
centroid *= 1.f/(float(result.mHullVcount) );
}
// collect vertices
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
if(m_transformSubShapes)
{
vertex -= centroid ;
}
vertices.push_back(vertex);
}
// build convex shape
btCollisionShape* convexShape = new btConvexHullShape(
&(vertices[0].getX()),vertices.size(),sizeof(btVector3));
m_convexShapes.push_back(convexShape);
convexShape->setMargin(m_compoundShape->getMargin());
if(m_transformSubShapes)
{
btTransform trans;
trans.setIdentity();
trans.setOrigin(centroid);
// add convex shape
m_compoundShape->addChildShape(trans,convexShape);
}
else
{
btTransform trans;
trans.setIdentity();
//trans.setOrigin(centroid);
// add convex shape
m_compoundShape->addChildShape(trans,convexShape);
//m_compoundShape->addChildShape(convexShape);
}
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->addTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
// float mass = 1.f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
convexShape->setMargin(0.01f);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper)
{
m_data = new ImportSDFInternalData;
(void)option;
// if (option==1)
// {
m_useMultiBody = true;
//
// } else
// {
// m_useMultiBody = false;
// }
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
{
gFileNameArray.clear();
//load additional urdf file names from file
FILE* f = fopen("sdf_files.txt","r");
if (f)
{
int result;
//warning: we don't avoid string buffer overflow in this basic example in fscanf
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("sdf_files.txt entry %s",fileName);
if (result==1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
fclose(f);
}
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("two_cubes.sdf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
{
count=0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
}
}
void InitShaders()
{
btOverlappingPairCache* overlappingPairCache=0;
int maxObjects = btMax(256,NUM_OBJECTS);
#ifdef USE_NEW
int maxPairsSmallProxy = 32;
sBroadphase = new btGridBroadphaseCl(overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128,
g_cxMainContext ,g_device,g_cqCommandQue, g_deviceCL);
#else
sBroadphase = new btGpu3DGridBroadphase(btVector3(2.f, 2.f, 2.f), 32, 32, 32,maxObjects, maxObjects, 64, 100.f, 64);
#endif
// sBroadphase = new bt3dGridBroadphaseOCL(overlappingPairCache,btVector3(10.f, 10.f, 10.f), 32, 32, 32,NUM_OBJECTS, NUM_OBJECTS, 64, 100.f, 16,
// g_cxMainContext ,g_device,g_cqCommandQue);
bool loadFromFile = false;
instancingShader = gltLoadShaderPair("instancing.vs","instancing.fs", loadFromFile);
glLinkProgram(instancingShader);
glUseProgram(instancingShader);
angle_loc = glGetUniformLocation(instancingShader, "angle");
ModelViewMatrix = glGetUniformLocation(instancingShader, "ModelViewMatrix");
ProjectionMatrix = glGetUniformLocation(instancingShader, "ProjectionMatrix");
uniform_texture_diffuse = glGetUniformLocation(instancingShader, "Diffuse");
GLuint offset = 0;
glGenBuffers(1, &cube_vbo);
glBindBuffer(GL_ARRAY_BUFFER, cube_vbo);
instance_positions_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_quaternion_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_colors_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_scale_ptr = (GLfloat*)new float[NUM_OBJECTS*3];
int index=0;
for (int i=0;i<NUM_OBJECTS_X;i++)
{
for (int j=0;j<NUM_OBJECTS_Y;j++)
{
for (int k=0;k<NUM_OBJECTS_Z;k++)
{
instance_positions_ptr[index*4]=(i*X_GAP-NUM_OBJECTS_X/2);
instance_positions_ptr[index*4+1]=(j*Y_GAP-NUM_OBJECTS_Y/2);
instance_positions_ptr[index*4+2]=(k*Z_GAP-NUM_OBJECTS_Z/2)+(j&1);
instance_positions_ptr[index*4+3]=1;
int shapeType =0;
void* userPtr = 0;
btVector3 aabbMin(
instance_positions_ptr[index*4],
instance_positions_ptr[index*4+1],
instance_positions_ptr[index*4+2]);
btVector3 aabbMax = aabbMin;
aabbMin -= btVector3(1.f,1.f,1.f);
aabbMax += btVector3(1.f,1.f,1.f);
void* myptr = (void*)index;//0;//&mBoxes[i]
btBroadphaseProxy* proxy = sBroadphase->createProxy(aabbMin,aabbMax,shapeType,myptr,1,1,0,0);//m_dispatcher);
proxyArray.push_back(proxy);
instance_quaternion_ptr[index*4]=0;
instance_quaternion_ptr[index*4+1]=0;
instance_quaternion_ptr[index*4+2]=0;
instance_quaternion_ptr[index*4+3]=1;
instance_colors_ptr[index*4]=j<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+1]=k<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+2]=i<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+3]=1.f;
instance_scale_ptr[index*3] = 1;
instance_scale_ptr[index*3+1] = 1;
instance_scale_ptr[index*3+2] = 1;
float mass = 1.f;//j? 1.f : 0.f;
bool writeToGpu = false;
if (narrowphaseAndSolver)
narrowphaseAndSolver->registerRigidBody(gShapeIndex,mass,&instance_positions_ptr[index*4],&instance_quaternion_ptr[index*4],writeToGpu);
index++;
}
}
}
float posZero[4] = {0,-NUM_OBJECTS_Y/2-1,0,0};
float ornZero[4] = {0,0,0,1};
//register a 'plane'
if (narrowphaseAndSolver)
narrowphaseAndSolver->registerRigidBody(-1, 0.f, posZero,ornZero,false);
if (narrowphaseAndSolver)
narrowphaseAndSolver->writeAllBodiesToGpu();
int size = sizeof(cube_vertices) + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE+SCALE_BUFFER_SIZE;
VBOsize = size;
char* bla = (char*)malloc(size);
int szc = sizeof(cube_vertices);
memcpy(bla,&cube_vertices[0],szc);
memcpy(bla+sizeof(cube_vertices),instance_positions_ptr,POSITION_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE,instance_quaternion_ptr,ORIENTATION_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE,instance_colors_ptr, COLOR_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,instance_scale_ptr, SCALE_BUFFER_SIZE);
glBufferData(GL_ARRAY_BUFFER, size, bla, GL_DYNAMIC_DRAW);//GL_STATIC_DRAW);
///initialize parts of the buffer
#ifdef _USE_SUB_DATA
glBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(cube_vertices)+ 16384, bla);//cube_vertices);
#endif
char* dest= (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
memcpy(dest,cube_vertices,sizeof(cube_vertices));
//memcpy(dest+sizeof(cube_vertices),instance_colors,sizeof(instance_colors));
glUnmapBuffer( GL_ARRAY_BUFFER);
writeTransforms();
/*
glBufferSubData(GL_ARRAY_BUFFER, sizeof(cube_vertices) + sizeof(instance_colors), POSITION_BUFFER_SIZE, instance_positions_ptr);
glBufferSubData(GL_ARRAY_BUFFER, sizeof(cube_vertices) + sizeof(instance_colors)+POSITION_BUFFER_SIZE,ORIENTATION_BUFFER_SIZE , instance_quaternion_ptr);
*/
glGenVertexArrays(1, &cube_vao);
glBindVertexArray(cube_vao);
glBindBuffer(GL_ARRAY_BUFFER, cube_vbo);
glBindVertexArray(0);
glGenBuffers(1, &index_vbo);
int indexBufferSize = sizeof(cube_indices);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_vbo);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indexBufferSize, NULL, GL_STATIC_DRAW);
glBufferSubData(GL_ELEMENT_ARRAY_BUFFER,0,indexBufferSize,cube_indices);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
glBindBuffer(GL_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
//--------------------------------------------------------------------------------------
// Release D3D11 resources created in OnD3D11CreateDevice
//--------------------------------------------------------------------------------------
void CALLBACK OnD3D11DestroyDevice( void* pUserContext )
{
g_DialogResourceManager.OnD3D11DestroyDevice();
g_D3DSettingsDlg.OnD3D11DestroyDevice();
DXUTGetGlobalResourceCache().OnDestroyDevice();
SAFE_DELETE( g_pTxtHelper );
g_Mesh11.Destroy();
SAFE_RELEASE(g_pGeometryShader);
SAFE_RELEASE( g_pVertexLayout11 );
SAFE_RELEASE( g_pVertexBuffer );
SAFE_RELEASE( g_pVertexShader );
SAFE_RELEASE( g_pPixelShader );
SAFE_RELEASE( g_pSamLinear );
SAFE_RELEASE( g_pcbVSPerObject );
SAFE_RELEASE( g_pcbPSPerObject );
SAFE_RELEASE( g_pcbPSPerFrame );
SAFE_RELEASE( g_pRasterizerState );
SAFE_RELEASE( g_pRasterizerStateWF );
for( int flagIndex = 0; flagIndex < numFlags; ++flagIndex )
{
cloths[flagIndex].destroy();
}
my_capsule.destroy();
// Shouldn't need to delete this as it's just a soft body and will be deleted later by the collision object cleanup.
//for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
//{
//delete m_flags[flagIndex];
//}
//cleanup in the reverse order of creation/initialization
if( g_defaultSolver )
delete g_defaultSolver;
if( g_cpuSolver )
delete g_cpuSolver;
if( g_dx11Solver )
delete g_dx11Solver;
if( g_dx11SIMDSolver )
delete g_dx11SIMDSolver;
if( g_softBodyOutput )
delete g_softBodyOutput;
for(int i=0; i< m_collisionShapes.size(); i++)
delete m_collisionShapes[i];
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject( obj );
delete obj;
}
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
void ImportUrdfSetup::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(2);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8;
m_dynamicsWorld->setGravity(gravity);
//now print the tree using the new interface
URDFImporterInterface* bla=0;
static bool newURDF = true;
if (newURDF)
{
b3Printf("using new URDF\n");
bla = new BulletURDFImporter(m_guiHelper);
}
#ifdef USE_ROS_URDF
else
{
b3Printf("using ROS URDF\n");
bla = new ROSURDFImporter(m_guiHelper);
}
newURDF = !newURDF;
#endif//USE_ROS_URDF
URDFImporterInterface& u2b = *bla;
bool loadOk = u2b.loadURDF(m_fileName);
#ifdef TEST_MULTIBODY_SERIALIZATION
//test to serialize a multibody to disk or shared memory, with base, link and joint names
btSerializer* s = new btDefaultSerializer;
#endif //TEST_MULTIBODY_SERIALIZATION
if (loadOk)
{
//printTree(u2b,u2b.getRootLinkIndex());
//u2b.printTree();
btTransform identityTrans;
identityTrans.setIdentity();
{
btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
mb = creation.getBulletMultiBody();
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 10.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
} else
{
if (1)
{
//create motors for each generic joint
int num6Dof = creation.getNum6DofConstraints();
for (int i=0;i<num6Dof;i++)
{
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr())
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
(jointInfo->m_urdfJointType ==URDFContinuousJoint))
{
int urdfLinkIndex = jointInfo->m_urdfIndex;
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
bool motorOn = true;
c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
m_data->m_numMotors++;
}
}
}
}
}
}
//the btMultiBody support is work-in-progress :-)
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
}
bool createGround=true;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2;//.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5,0.5,0.5);
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}
#ifdef TEST_MULTIBODY_SERIALIZATION
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
#endif//TEST_MULTIBODY_SERIALIZATION
}
void initBullet(void)
{
#ifdef USE_GPU_SOLVER
g_dx11Solver = new btDX11SoftBodySolver( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
g_solver = g_dx11Solver;
#ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoDX( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
#else // #ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#endif // #ifdef USE_GPU_COPY
#else
#ifdef USE_SIMDAWARE_SOLVER
g_dx11SIMDSolver = new btDX11SIMDAwareSoftBodySolver( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
g_solver = g_dx11SIMDSolver;
g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoDX( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
#else // #ifdef USE_GPU_COPY
g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#endif // #ifdef USE_GPU_COPY
#else
g_cpuSolver = new btCPUSoftBodySolver;
g_solver = g_cpuSolver;
g_softBodyOutput = new btSoftBodySolverOutputCPUtoCPU;
//g_defaultSolver = new btDefaultSoftBodySolver;
//g_solver = g_defaultSolver;
#endif
#endif
if (g_dx11SIMDSolver)
g_dx11SIMDSolver->setEnableUpdateBounds(true);
if (g_dx11Solver)
g_dx11Solver->setEnableUpdateBounds(true);
// Initialise CPU physics device
//m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, g_solver);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
m_dynamicsWorld->getWorldInfo().air_density = (btScalar)1.2;
m_dynamicsWorld->getWorldInfo().water_density = 0;
m_dynamicsWorld->getWorldInfo().water_offset = 0;
m_dynamicsWorld->getWorldInfo().water_normal = btVector3(0,0,0);
m_dynamicsWorld->getWorldInfo().m_gravity.setValue(0,-10,0);
#if 0
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
#endif
#if 0
{
btScalar mass(0.);
//btScalar mass(1.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btCollisionShape *capsuleShape = new btCapsuleShape(5, 10);
capsuleShape->setMargin( 0.5 );
my_capsule.set_collision_shape(capsuleShape);
btVector3 localInertia(0,0,0);
if (isDynamic)
capsuleShape->calculateLocalInertia(mass,localInertia);
m_collisionShapes.push_back(capsuleShape);
btTransform capsuleTransform;
capsuleTransform.setIdentity();
#ifdef TABLETEST
capsuleTransform.setOrigin(btVector3(0, 10, -11));
const btScalar pi = 3.141592654;
capsuleTransform.setRotation(btQuaternion(0, 0, pi/2));
#else
capsuleTransform.setOrigin(btVector3(0, 20, -10));
const btScalar pi = 3.141592654;
//capsuleTransform.setRotation(btQuaternion(0, 0, pi/2));
capsuleTransform.setRotation(btQuaternion(0, 0, 0));
#endif
btDefaultMotionState* myMotionState = new btDefaultMotionState(capsuleTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,capsuleShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction( 0.8f );
my_capsule.set_collision_object(body);
m_dynamicsWorld->addRigidBody(body);
//cap_1.collisionShape = body;
capCollider = body;
}
#endif
createFlag( clothWidth, clothHeight, m_flags );
// Create output buffer descriptions for ecah flag
// These describe where the simulation should send output data to
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
// In this case we have a DX11 output buffer with a vertex at index 0, 8, 16 and so on as well as a normal at 3, 11, 19 etc.
// Copies will be performed GPU-side directly into the output buffer
#ifdef USE_GPU_COPY
btDX11VertexBufferDescriptor *vertexBufferDescriptor = new btDX11VertexBufferDescriptor(DXUTGetD3D11DeviceContext(), cloths[flagIndex].pVB[0], cloths[flagIndex].g_pVB_UAV, 0, 8, 3, 8);
cloths[flagIndex].m_vertexBufferDescriptor = vertexBufferDescriptor;
#else // #ifdef USE_GPU_COPY
btCPUVertexBufferDescriptor *vertexBufferDescriptor = new btCPUVertexBufferDescriptor(cloths[flagIndex].cpu_buffer, 0, 8, 3, 8);
cloths[flagIndex].m_vertexBufferDescriptor = vertexBufferDescriptor;
#endif // #ifdef USE_GPU_COPY
}
g_solver->optimize( m_dynamicsWorld->getSoftBodyArray() );
}
void btOpenCLSoftBodySolverSIMDAware::optimize( btAlignedObjectArray< btSoftBody * > &softBodies ,bool forceUpdate)
{
if( forceUpdate || m_softBodySet.size() != softBodies.size() )
{
// Have a change in the soft body set so update, reloading all the data
getVertexData().clear();
getTriangleData().clear();
getLinkData().clear();
m_softBodySet.resize(0);
m_anchorIndex.clear();
int maxPiterations = 0;
int maxViterations = 0;
for( int softBodyIndex = 0; softBodyIndex < softBodies.size(); ++softBodyIndex )
{
btSoftBody *softBody = softBodies[ softBodyIndex ];
using Vectormath::Aos::Matrix3;
using Vectormath::Aos::Point3;
// Create SoftBody that will store the information within the solver
btOpenCLAcceleratedSoftBodyInterface* newSoftBody = new btOpenCLAcceleratedSoftBodyInterface( softBody );
m_softBodySet.push_back( newSoftBody );
m_perClothAcceleration.push_back( toVector3(softBody->getWorldInfo()->m_gravity) );
m_perClothDampingFactor.push_back(softBody->m_cfg.kDP);
m_perClothVelocityCorrectionCoefficient.push_back( softBody->m_cfg.kVCF );
m_perClothLiftFactor.push_back( softBody->m_cfg.kLF );
m_perClothDragFactor.push_back( softBody->m_cfg.kDG );
m_perClothMediumDensity.push_back(softBody->getWorldInfo()->air_density);
// Simple init values. Actually we'll put 0 and -1 into them at the appropriate time
m_perClothFriction.push_back(softBody->m_cfg.kDF);
m_perClothCollisionObjects.push_back( CollisionObjectIndices(-1, -1) );
// Add space for new vertices and triangles in the default solver for now
// TODO: Include space here for tearing too later
int firstVertex = getVertexData().getNumVertices();
int numVertices = softBody->m_nodes.size();
// Round maxVertices to a multiple of the workgroup size so we know we're safe to run over in a given group
// maxVertices can be increased to allow tearing, but should be used sparingly because these extra verts will always be processed
int maxVertices = GROUP_SIZE*((numVertices+GROUP_SIZE)/GROUP_SIZE);
// Allocate space for new vertices in all the vertex arrays
getVertexData().createVertices( numVertices, softBodyIndex, maxVertices );
int firstTriangle = getTriangleData().getNumTriangles();
int numTriangles = softBody->m_faces.size();
int maxTriangles = numTriangles;
getTriangleData().createTriangles( maxTriangles );
// Copy vertices from softbody into the solver
for( int vertex = 0; vertex < numVertices; ++vertex )
{
Point3 multPoint(softBody->m_nodes[vertex].m_x.getX(), softBody->m_nodes[vertex].m_x.getY(), softBody->m_nodes[vertex].m_x.getZ());
btSoftBodyVertexData::VertexDescription desc;
// TODO: Position in the softbody might be pre-transformed
// or we may need to adapt for the pose.
//desc.setPosition( cloth.getMeshTransform()*multPoint );
desc.setPosition( multPoint );
float vertexInverseMass = softBody->m_nodes[vertex].m_im;
desc.setInverseMass(vertexInverseMass);
getVertexData().setVertexAt( desc, firstVertex + vertex );
m_anchorIndex.push_back(-1);
}
for( int vertex = numVertices; vertex < maxVertices; ++vertex )
{
m_anchorIndex.push_back(-1.0);
}
// Copy triangles similarly
// We're assuming here that vertex indices are based on the firstVertex rather than the entire scene
for( int triangle = 0; triangle < numTriangles; ++triangle )
{
// Note that large array storage is relative to the array not to the cloth
// So we need to add firstVertex to each value
int vertexIndex0 = (softBody->m_faces[triangle].m_n[0] - &(softBody->m_nodes[0]));
int vertexIndex1 = (softBody->m_faces[triangle].m_n[1] - &(softBody->m_nodes[0]));
int vertexIndex2 = (softBody->m_faces[triangle].m_n[2] - &(softBody->m_nodes[0]));
btSoftBodyTriangleData::TriangleDescription newTriangle(vertexIndex0 + firstVertex, vertexIndex1 + firstVertex, vertexIndex2 + firstVertex);
getTriangleData().setTriangleAt( newTriangle, firstTriangle + triangle );
// Increase vertex triangle counts for this triangle
getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex0)++;
getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex1)++;
getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex2)++;
}
int firstLink = getLinkData().getNumLinks();
int numLinks = softBody->m_links.size();
int maxLinks = numLinks;
// Allocate space for the links
getLinkData().createLinks( numLinks );
// Add the links
for( int link = 0; link < numLinks; ++link )
{
int vertexIndex0 = softBody->m_links[link].m_n[0] - &(softBody->m_nodes[0]);
int vertexIndex1 = softBody->m_links[link].m_n[1] - &(softBody->m_nodes[0]);
btSoftBodyLinkData::LinkDescription newLink(vertexIndex0 + firstVertex, vertexIndex1 + firstVertex, softBody->m_links[link].m_material->m_kLST);
newLink.setLinkStrength(1.f);
getLinkData().setLinkAt(newLink, firstLink + link);
}
newSoftBody->setFirstVertex( firstVertex );
newSoftBody->setFirstTriangle( firstTriangle );
newSoftBody->setNumVertices( numVertices );
newSoftBody->setMaxVertices( maxVertices );
newSoftBody->setNumTriangles( numTriangles );
newSoftBody->setMaxTriangles( maxTriangles );
newSoftBody->setFirstLink( firstLink );
newSoftBody->setNumLinks( numLinks );
// Find maximum piterations and viterations
int piterations = softBody->m_cfg.piterations;
if ( piterations > maxPiterations )
maxPiterations = piterations;
int viterations = softBody->m_cfg.viterations;
if ( viterations > maxViterations )
maxViterations = viterations;
// zero mass
for( int vertex = 0; vertex < numVertices; ++vertex )
{
if ( softBody->m_nodes[vertex].m_im == 0 )
{
AnchorNodeInfoCL nodeInfo;
nodeInfo.clVertexIndex = firstVertex + vertex;
nodeInfo.pNode = &softBody->m_nodes[vertex];
m_anchorNodeInfoArray.push_back(nodeInfo);
}
}
// anchor position
if ( numVertices > 0 )
{
for ( int anchorIndex = 0; anchorIndex < softBody->m_anchors.size(); anchorIndex++ )
{
btSoftBody::Node* anchorNode = softBody->m_anchors[anchorIndex].m_node;
btSoftBody::Node* firstNode = &softBody->m_nodes[0];
AnchorNodeInfoCL nodeInfo;
nodeInfo.clVertexIndex = firstVertex + (int)(anchorNode - firstNode);
nodeInfo.pNode = anchorNode;
m_anchorNodeInfoArray.push_back(nodeInfo);
}
}
}
m_anchorPosition.clear();
m_anchorPosition.resize(m_anchorNodeInfoArray.size());
for ( int anchorNode = 0; anchorNode < m_anchorNodeInfoArray.size(); anchorNode++ )
{
const AnchorNodeInfoCL& anchorNodeInfo = m_anchorNodeInfoArray[anchorNode];
m_anchorIndex[anchorNodeInfo.clVertexIndex] = anchorNode;
getVertexData().getInverseMass(anchorNodeInfo.clVertexIndex) = 0.0f;
}
updateConstants(0.f);
// set position and velocity iterations
setNumberOfPositionIterations(maxPiterations);
setNumberOfVelocityIterations(maxViterations);
// set wind velocity
m_perClothWindVelocity.resize( m_softBodySet.size() );
for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
{
btSoftBody *softBody = m_softBodySet[softBodyIndex]->getSoftBody();
m_perClothWindVelocity[softBodyIndex] = toVector3(softBody->getWindVelocity());
}
m_clPerClothWindVelocity.changedOnCPU();
// generate batches
m_linkData.generateBatches();
m_triangleData.generateBatches();
// Build the shaders to match the batching parameters
buildShaders();
}
}
virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)
{
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
btMultiBodyConstraint** startMultiBodyConstraint = 0;
int numCurConstraints = 0;
int numCurMultiBodyConstraints = 0;
int i;
//find the first constraint for this island
for (i=0;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (;i<m_numConstraints;i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
for (i=0;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
numCurMultiBodyConstraints++;
}
}
//if (m_solverInfo->m_minimumSolverBatchSize<=1)
//{
// m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
//} else
{
for (i=0;i<numBodies;i++)
m_bodies.push_back(bodies[i]);
for (i=0;i<numManifolds;i++)
m_manifolds.push_back(manifolds[i]);
for (i=0;i<numCurConstraints;i++)
m_constraints.push_back(startConstraint[i]);
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else
{
//printf("deferred\n");
}
}
}
}
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();
}
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;
}
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 );
}
}
}
}
void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling)
{
btHashMap<btHashString,TiXmlElement* > allSources;
btHashMap<btHashString,VertexSource> vertexSources;
for(TiXmlElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry");
geometry != NULL; geometry = geometry->NextSiblingElement("geometry"))
{
btAlignedObjectArray<btVector3> vertexPositions;
btAlignedObjectArray<btVector3> vertexNormals;
btAlignedObjectArray<int> indices;
const char* geometryName = geometry->Attribute("id");
for (TiXmlElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
{
TiXmlElement* vertices2 = mesh->FirstChildElement("vertices");
for (TiXmlElement* source = mesh->FirstChildElement("source");source != NULL;source = source->NextSiblingElement("source"))
{
const char* srcId= source->Attribute("id");
// printf("source id=%s\n",srcId);
allSources.insert(srcId,source);
}
const char* vertexId = vertices2->Attribute("id");
//printf("vertices id=%s\n",vertexId);
VertexSource vs;
for(TiXmlElement* input = vertices2->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
{
const char* sem = input->Attribute("semantic");
std::string semName(sem);
// printf("sem=%s\n",sem);
// const char* src = input->Attribute("source");
// printf("src=%s\n",src);
const char* srcIdRef = input->Attribute("source");
std::string source_name;
source_name = std::string(srcIdRef);
source_name = source_name.erase(0, 1);
if (semName=="POSITION")
{
vs.m_positionArrayId = source_name;
}
if (semName=="NORMAL")
{
vs.m_normalArrayId = source_name;
}
}
vertexSources.insert(vertexId,vs);
for (TiXmlElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles"))
{
std::string positionSourceName;
std::string normalSourceName;
int primitiveCount;
primitive->QueryIntAttribute("count", &primitiveCount);
int indexStride=1;
int posOffset = 0;
int normalOffset = 0;
int numIndices = 0;
{
for (TiXmlElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
{
const char* sem = input->Attribute("semantic");
std::string semName(sem);
int offset = atoi(input->Attribute("offset"));
if ((offset+1)>indexStride)
indexStride=offset+1;
//printf("sem=%s\n",sem);
// const char* src = input->Attribute("source");
//printf("src=%s\n",src);
const char* srcIdRef = input->Attribute("source");
std::string source_name;
source_name = std::string(srcIdRef);
source_name = source_name.erase(0, 1);
if (semName=="VERTEX")
{
//now we have POSITION and possibly NORMAL too, using same index array (<p>)
VertexSource* vs = vertexSources[source_name.c_str()];
if (vs->m_positionArrayId.length())
{
positionSourceName = vs->m_positionArrayId;
posOffset = offset;
}
if (vs->m_normalArrayId.length())
{
normalSourceName = vs->m_normalArrayId;
normalOffset = offset;
}
}
if (semName=="NORMAL")
{
btAssert(normalSourceName.length()==0);
normalSourceName = source_name;
normalOffset = offset;
}
}
numIndices = primitiveCount * 3;
}
btAlignedObjectArray<float> positionFloatArray;
int posStride=1;
TiXmlElement** sourcePtr = allSources[positionSourceName.c_str()];
if (sourcePtr)
{
readFloatArray(*sourcePtr,positionFloatArray, posStride);
}
btAlignedObjectArray<float> normalFloatArray;
int normalStride=1;
sourcePtr = allSources[normalSourceName.c_str()];
if (sourcePtr)
{
readFloatArray(*sourcePtr,normalFloatArray,normalStride);
}
btAlignedObjectArray<int> curIndices;
curIndices.reserve(numIndices*indexStride);
TokenIntArray adder(curIndices);
tokenize(primitive->FirstChildElement("p")->GetText(),adder);
assert(curIndices.size() == numIndices*indexStride);
int indexOffset = vertexPositions.size();
for(int index=0; index<numIndices; index++)
{
int posIndex = curIndices[index*indexStride+posOffset];
int normalIndex = curIndices[index*indexStride+normalOffset];
vertexPositions.push_back(btVector3(extraScaling*positionFloatArray[posIndex*3+0],
extraScaling*positionFloatArray[posIndex*3+1],
extraScaling*positionFloatArray[posIndex*3+2]));
if (normalFloatArray.size() && (normalFloatArray.size()>normalIndex))
{
vertexNormals.push_back(btVector3(normalFloatArray[normalIndex*3+0],
normalFloatArray[normalIndex*3+1],
normalFloatArray[normalIndex*3+2]));
} else
{
//add a dummy normal of length zero, so it is easy to detect that it is an invalid normal
vertexNormals.push_back(btVector3(0,0,0));
}
}
int curNumIndices = indices.size();
indices.resize(curNumIndices+numIndices);
for(int index=0; index<numIndices; index++)
{
indices[curNumIndices+index] = index+indexOffset;
}
}//if(primitive != NULL)
}//for each mesh
int shapeIndex = visualShapes.size();
if (shapeIndex<MAX_VISUAL_SHAPES)
{
GLInstanceGraphicsShape& visualShape = visualShapes.expand();
{
visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
visualShape.m_indices = new b3AlignedObjectArray<int>;
int indexBase = 0;
btAssert(vertexNormals.size()==vertexPositions.size());
for (int v=0;v<vertexPositions.size();v++)
{
GLInstanceVertex vtx;
vtx.xyzw[0] = vertexPositions[v].x();
vtx.xyzw[1] = vertexPositions[v].y();
vtx.xyzw[2] = vertexPositions[v].z();
vtx.xyzw[3] = 1.f;
vtx.normal[0] = vertexNormals[v].x();
vtx.normal[1] = vertexNormals[v].y();
vtx.normal[2] = vertexNormals[v].z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
visualShape.m_vertices->push_back(vtx);
}
for (int index=0;index<indices.size();index++)
{
visualShape.m_indices->push_back(indices[index]+indexBase);
}
//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
indexBase=visualShape.m_vertices->size();
visualShape.m_numIndices = visualShape.m_indices->size();
visualShape.m_numvertices = visualShape.m_vertices->size();
}
//b3Printf("geometry name=%s\n",geometryName);
name2Shape.insert(geometryName,shapeIndex);
} else
{
b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES);
}
}//for each geometry
}
bool btConvexUtility::initializePolyhedralFeatures(const btAlignedObjectArray<btVector3>& orgVertices, bool mergeCoplanarTriangles)
{
btConvexHullComputer conv;
conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
btAlignedObjectArray<btFace> tmpFaces;
tmpFaces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
btVector3 edges[3];
int numEdges = 0;
//compute face normals
btScalar maxCross2 = 0.f;
int chosenEdge = -1;
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_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
tmpFaces[i].m_plane[3] = -planeEq;
}
//merge coplanar faces
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--)
{
int i = todoFaces[j];
btFace& faceB = tmpFaces[i];
btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
{
coplanarFaceGroup.push_back(i);
todoFaces.remove(i);
}
}
bool did_merge = false;
if (mergeCoplanarTriangles && coplanarFaceGroup.size()>1)
{
//do the merge: use Graham Scan 2d convex hull
btAlignedObjectArray<GrahamVector2> orgpoints;
for (int i=0;i<coplanarFaceGroup.size();i++)
{
btFace& face = tmpFaces[coplanarFaceGroup[i]];
btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
btVector3 xyPlaneNormal(0,0,1);
btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal);
for (int f=0;f<face.m_indices.size();f++)
{
int orgIndex = face.m_indices[f];
btVector3 pt = m_vertices[orgIndex];
btVector3 rotatedPt = quatRotate(rotationArc,pt);
rotatedPt.setZ(0);
bool found = false;
for (int i=0;i<orgpoints.size();i++)
{
//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
if (orgpoints[i].m_orgIndex == orgIndex)
{
found=true;
break;
}
}
if (!found)
orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex));
}
}
btFace combinedFace;
for (int i=0;i<4;i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
btAlignedObjectArray<GrahamVector2> hull;
GrahamScanConvexHull2D(orgpoints,hull);
for (int i=0;i<hull.size();i++)
{
combinedFace.m_indices.push_back(hull[i].m_orgIndex);
for(int k = 0; k < orgpoints.size(); k++) {
if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) {
orgpoints[k].m_orgIndex = -1; // invalidate...
break;
}
}
}
// are there rejected vertices?
bool reject_merge = false;
for(int i = 0; i < orgpoints.size(); i++) {
if(orgpoints[i].m_orgIndex == -1)
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for(int j = 0; j < tmpFaces.size(); j++) {
btFace& face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
for(int k = 0; k < coplanarFaceGroup.size(); k++) {
if(coplanarFaceGroup[k] == j) {
is_in_current_group = true;
break;
}
}
if(is_in_current_group) // ignore this face...
continue;
// does this face use this rejected vertex?
for(int v = 0; v < face.m_indices.size(); v++) {
if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
// this rejected vertex is used in another face -- reject merge
reject_merge = true;
break;
}
}
if(reject_merge)
break;
}
if(reject_merge)
break;
}
if(!reject_merge) {
// do this merge!
did_merge = true;
m_faces.push_back(combinedFace);
}
}
if(!did_merge)
{
for (int i=0;i<coplanarFaceGroup.size();i++)
{
m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
}
}
}
return true;
}
void ExampleEntries::registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option)
{
ExampleEntry e( menuLevel,name,description, createFunc, option);
gAdditionalRegisteredExamples.push_back(e);
}
void TestJointTorqueSetup::initPhysics()
{
int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
{
bool floating = false;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for(int i = 0; i < numLinks; ++i)
{
float linkMass = 1.f;
//if (i==3 || i==2)
// linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = 0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
} else
{
shape = new btSphereShape(radius);
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
if(!spherical)
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
} else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody->finalizeMultiDof();
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<pMultiBody->getNumLinks();i++)
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
pMultiBody->getLink(i).m_jointFeedback = fb;
m_jointFeedbacks.push_back(fb);
//break;
}
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
//////////////////////////////////////////////
if(0)//numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
// double friction = 1;
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
// btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
{
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating);
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
{
shape = new btSphereShape(radius);
}
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0);
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
//if (i==0||i>numLinks-2)
{
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
pMultiBody->getLink(i).m_collider=col;
}
}
}
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
char resourcePath[1024];
if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
}