mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-11-16 20:56:04 +00:00
Main improvements are: GPU cloth collision detection against a capsule shape ,OpenCL-OpenGL interoperability (keeping data buffers on GPU), and bug fixes Thanks to Lee Howes
980 lines
39 KiB
C++
980 lines
39 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
|
|
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
|
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
|
|
#include "vectormath/vmInclude.h"
|
|
|
|
#include "BulletMultiThreaded/GpuSoftBodySolvers/CPU/btSoftBodySolver_CPU.h"
|
|
#include "BulletSoftBody/btSoftBody.h"
|
|
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
|
|
|
|
|
|
btCPUSoftBodySolver::btCPUSoftBodySolver()
|
|
{
|
|
// Initial we will clearly need to update solver constants
|
|
// For now this is global for the cloths linked with this solver - we should probably make this body specific
|
|
// for performance in future once we understand more clearly when constants need to be updated
|
|
m_updateSolverConstants = true;
|
|
}
|
|
|
|
btCPUSoftBodySolver::~btCPUSoftBodySolver()
|
|
{
|
|
}
|
|
|
|
|
|
|
|
|
|
btSoftBodyLinkData &btCPUSoftBodySolver::getLinkData()
|
|
{
|
|
return m_linkData;
|
|
}
|
|
|
|
btSoftBodyVertexData &btCPUSoftBodySolver::getVertexData()
|
|
{
|
|
return m_vertexData;
|
|
}
|
|
|
|
btSoftBodyTriangleData &btCPUSoftBodySolver::getTriangleData()
|
|
{
|
|
return m_triangleData;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static Vectormath::Aos::Vector3 toVector3( const btVector3 &vec )
|
|
{
|
|
Vectormath::Aos::Vector3 outVec( vec.getX(), vec.getY(), vec.getZ() );
|
|
return outVec;
|
|
}
|
|
|
|
static Vectormath::Aos::Transform3 toTransform3( const btTransform &transform )
|
|
{
|
|
Vectormath::Aos::Transform3 outTransform;
|
|
outTransform.setCol(0, toVector3(transform.getBasis().getColumn(0)));
|
|
outTransform.setCol(1, toVector3(transform.getBasis().getColumn(1)));
|
|
outTransform.setCol(2, toVector3(transform.getBasis().getColumn(2)));
|
|
outTransform.setCol(3, toVector3(transform.getOrigin()));
|
|
return outTransform;
|
|
}
|
|
|
|
void btCPUSoftBodySolver::btAcceleratedSoftBodyInterface::updateBounds( const btVector3 &lowerBound, const btVector3 &upperBound )
|
|
{
|
|
float scalarMargin = this->getSoftBody()->getCollisionShape()->getMargin();
|
|
btVector3 vectorMargin( scalarMargin, scalarMargin, scalarMargin );
|
|
m_softBody->m_bounds[0] = lowerBound - vectorMargin;
|
|
m_softBody->m_bounds[1] = upperBound + vectorMargin;
|
|
}
|
|
|
|
|
|
void btCPUSoftBodySolver::copyBackToSoftBodies()
|
|
{
|
|
// Loop over soft bodies, copying all the vertex positions back for each body in turn
|
|
for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *softBodyInterface = m_softBodySet[ softBodyIndex ];
|
|
btSoftBody *softBody = softBodyInterface->getSoftBody();
|
|
|
|
int firstVertex = softBodyInterface->getFirstVertex();
|
|
int numVertices = softBodyInterface->getNumVertices();
|
|
|
|
// Copy vertices from solver back into the softbody
|
|
for( int vertex = 0; vertex < numVertices; ++vertex )
|
|
{
|
|
using Vectormath::Aos::Point3;
|
|
Point3 vertexPosition( getVertexData().getVertexPositions()[firstVertex + vertex] );
|
|
|
|
softBody->m_nodes[vertex].m_x.setX( vertexPosition.getX() );
|
|
softBody->m_nodes[vertex].m_x.setY( vertexPosition.getY() );
|
|
softBody->m_nodes[vertex].m_x.setZ( vertexPosition.getZ() );
|
|
|
|
softBody->m_nodes[vertex].m_n.setX( vertexPosition.getX() );
|
|
softBody->m_nodes[vertex].m_n.setY( vertexPosition.getY() );
|
|
softBody->m_nodes[vertex].m_n.setZ( vertexPosition.getZ() );
|
|
}
|
|
}
|
|
} // btCPUSoftBodySolver::copyBackToSoftBodies
|
|
|
|
void btCPUSoftBodySolver::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);
|
|
|
|
|
|
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
|
|
btAcceleratedSoftBodyInterface *newSoftBody = new btAcceleratedSoftBodyInterface( 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);
|
|
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();
|
|
int maxVertices = numVertices;
|
|
// Allocate space for new vertices in all the vertex arrays
|
|
getVertexData().createVertices( maxVertices, softBodyIndex );
|
|
|
|
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 );
|
|
}
|
|
|
|
// 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 );
|
|
}
|
|
|
|
|
|
|
|
updateConstants(0.f);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void btCPUSoftBodySolver::updateSoftBodies()
|
|
{
|
|
using namespace Vectormath::Aos;
|
|
|
|
int numVertices = m_vertexData.getNumVertices();
|
|
int numTriangles = m_triangleData.getNumTriangles();
|
|
|
|
// Initialise normal and vertex counts
|
|
for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
|
|
{
|
|
m_vertexData.getArea(vertexIndex) = 0.f;
|
|
m_vertexData.getNormal(vertexIndex) = Vector3(0.f, 0.f, 0.f);
|
|
}
|
|
|
|
// Update the areas for the triangles and vertices.
|
|
for( int triangleIndex = 0; triangleIndex < numTriangles; ++triangleIndex )
|
|
{
|
|
float &triangleArea( m_triangleData.getTriangleArea( triangleIndex ) );
|
|
const btSoftBodyTriangleData::TriangleNodeSet &vertices( m_triangleData.getVertexSet(triangleIndex) );
|
|
|
|
Point3 &vertexPosition0( m_vertexData.getPosition( vertices.vertex0 ) );
|
|
Point3 &vertexPosition1( m_vertexData.getPosition( vertices.vertex1 ) );
|
|
Point3 &vertexPosition2( m_vertexData.getPosition( vertices.vertex2 ) );
|
|
|
|
triangleArea = computeTriangleArea( vertexPosition0, vertexPosition1, vertexPosition2 );
|
|
|
|
// Add to areas for vertices and increase the count of the number of triangles affecting the vertex
|
|
m_vertexData.getArea(vertices.vertex0) += triangleArea;
|
|
m_vertexData.getArea(vertices.vertex1) += triangleArea;
|
|
m_vertexData.getArea(vertices.vertex2) += triangleArea;
|
|
|
|
Point3 &vertex0( m_vertexData.getPosition(vertices.vertex0) );
|
|
Point3 &vertex1( m_vertexData.getPosition(vertices.vertex1) );
|
|
Point3 &vertex2( m_vertexData.getPosition(vertices.vertex2) );
|
|
|
|
Vector3 triangleNormal = cross( vertex1-vertex0, vertex2 - vertex0 );
|
|
|
|
m_triangleData.getNormal(triangleIndex) = normalize(triangleNormal);
|
|
|
|
m_vertexData.getNormal(vertices.vertex0) += triangleNormal;
|
|
m_vertexData.getNormal(vertices.vertex1) += triangleNormal;
|
|
m_vertexData.getNormal(vertices.vertex2) += triangleNormal;
|
|
|
|
}
|
|
|
|
// Normalise the area and normals
|
|
for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
|
|
{
|
|
m_vertexData.getArea(vertexIndex) /= m_vertexData.getTriangleCount(vertexIndex);
|
|
m_vertexData.getNormal(vertexIndex) = normalize( m_vertexData.getNormal(vertexIndex) );
|
|
}
|
|
|
|
|
|
// Clear the collision shape array for the next frame
|
|
m_collisionObjectDetails.clear();
|
|
|
|
} // updateSoftBodies
|
|
|
|
|
|
Vectormath::Aos::Vector3 btCPUSoftBodySolver::ProjectOnAxis( const Vectormath::Aos::Vector3 &v, const Vectormath::Aos::Vector3 &a )
|
|
{
|
|
return a*Vectormath::Aos::dot(v, a);
|
|
}
|
|
|
|
void btCPUSoftBodySolver::ApplyClampedForce( float solverdt, const Vectormath::Aos::Vector3 &force, const Vectormath::Aos::Vector3 &vertexVelocity, float inverseMass, Vectormath::Aos::Vector3 &vertexForce )
|
|
{
|
|
float dtInverseMass = solverdt*inverseMass;
|
|
if( Vectormath::Aos::lengthSqr(force * dtInverseMass) > Vectormath::Aos::lengthSqr(vertexVelocity) )
|
|
{
|
|
vertexForce -= ProjectOnAxis( vertexVelocity, normalize( force ) )/dtInverseMass;
|
|
} else {
|
|
vertexForce += force;
|
|
}
|
|
}
|
|
|
|
bool btCPUSoftBodySolver::checkInitialized()
|
|
{
|
|
return true;
|
|
}
|
|
|
|
void btCPUSoftBodySolver::applyForces( float solverdt )
|
|
{
|
|
using namespace Vectormath::Aos;
|
|
|
|
int numVertices = m_vertexData.getNumVertices();
|
|
for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
|
|
const int startVertex = currentCloth->getFirstVertex();
|
|
const int numVertices = currentCloth->getNumVertices();
|
|
|
|
Vector3 velocityChange = m_perClothAcceleration[clothIndex]*solverdt;
|
|
for( int vertexIndex = startVertex; vertexIndex < (startVertex + numVertices); ++vertexIndex )
|
|
{
|
|
float inverseMass = m_vertexData.getInverseMass( vertexIndex );
|
|
Vector3 &vertexVelocity( m_vertexData.getVelocity( vertexIndex ) );
|
|
|
|
// First apply the global acceleration to all vertices
|
|
if( inverseMass > 0 )
|
|
vertexVelocity += velocityChange;
|
|
|
|
// If it's a non-static vertex
|
|
if( m_vertexData.getInverseMass(vertexIndex) > 0 )
|
|
{
|
|
// Wind effects on a wind-per-cloth basis
|
|
float liftFactor = m_perClothLiftFactor[clothIndex];
|
|
float dragFactor = m_perClothDragFactor[clothIndex];
|
|
if( (liftFactor > 0.f) || (dragFactor > 0.f) )
|
|
{
|
|
Vector3 normal = m_vertexData.getNormal(vertexIndex);
|
|
Vector3 relativeWindVelocity = m_vertexData.getVelocity(vertexIndex) - m_perClothWindVelocity[clothIndex];
|
|
float relativeSpeedSquared = lengthSqr(relativeWindVelocity);
|
|
if( relativeSpeedSquared > FLT_EPSILON )
|
|
{
|
|
normal = normal * (dot(normal, relativeWindVelocity) < 0 ? -1.f : +1.f);
|
|
float dvNormal = dot(normal, relativeWindVelocity);
|
|
if( dvNormal > 0 )
|
|
{
|
|
Vector3 force( 0.f, 0.f, 0.f );
|
|
float c0 = m_vertexData.getArea(vertexIndex) * dvNormal * relativeSpeedSquared / 2;
|
|
float c1 = c0 * m_perClothMediumDensity[clothIndex];
|
|
force += normal * (-c1 * liftFactor);
|
|
force += normalize(relativeWindVelocity)*(-c1 * dragFactor);
|
|
|
|
Vectormath::Aos::Vector3 &vertexForce( m_vertexData.getForceAccumulator(vertexIndex) );
|
|
ApplyClampedForce( solverdt, force, vertexVelocity, inverseMass, vertexForce );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} // btCPUSoftBodySolver::applyForces
|
|
|
|
/**
|
|
* Integrate motion on the solver.
|
|
*/
|
|
void btCPUSoftBodySolver::integrate( float solverdt )
|
|
{
|
|
using namespace Vectormath::Aos;
|
|
int numVertices = m_vertexData.getNumVertices();
|
|
for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
|
|
{
|
|
Point3 &position( m_vertexData.getPosition(vertexIndex) );
|
|
Point3 &previousPosition( m_vertexData.getPreviousPosition(vertexIndex) );
|
|
Vector3 &forceAccumulator( m_vertexData.getForceAccumulator(vertexIndex) );
|
|
Vector3 &velocity( m_vertexData.getVelocity(vertexIndex) );
|
|
float inverseMass = m_vertexData.getInverseMass(vertexIndex);
|
|
|
|
previousPosition = position;
|
|
velocity += forceAccumulator * inverseMass * solverdt;
|
|
position += velocity * solverdt;
|
|
forceAccumulator = Vector3(0.f, 0.f, 0.f);
|
|
}
|
|
} // btCPUSoftBodySolver::integrate
|
|
|
|
float btCPUSoftBodySolver::computeTriangleArea(
|
|
const Vectormath::Aos::Point3 &vertex0,
|
|
const Vectormath::Aos::Point3 &vertex1,
|
|
const Vectormath::Aos::Point3 &vertex2 )
|
|
{
|
|
Vectormath::Aos::Vector3 a = vertex1 - vertex0;
|
|
Vectormath::Aos::Vector3 b = vertex2 - vertex0;
|
|
Vectormath::Aos::Vector3 crossProduct = cross(a, b);
|
|
float area = length( crossProduct );
|
|
return area;
|
|
}
|
|
|
|
void btCPUSoftBodySolver::updateConstants( float timeStep )
|
|
{
|
|
using namespace Vectormath::Aos;
|
|
|
|
if( m_updateSolverConstants )
|
|
{
|
|
m_updateSolverConstants = false;
|
|
|
|
// Will have to redo this if we change the structure (tear, maybe) or various other possible changes
|
|
|
|
// Initialise link constants
|
|
const int numLinks = m_linkData.getNumLinks();
|
|
for( int linkIndex = 0; linkIndex < numLinks; ++linkIndex )
|
|
{
|
|
btSoftBodyLinkData::LinkNodePair &vertices( m_linkData.getVertexPair(linkIndex) );
|
|
m_linkData.getRestLength(linkIndex) = length((m_vertexData.getPosition( vertices.vertex0 ) - m_vertexData.getPosition( vertices.vertex1 )));
|
|
float invMass0 = m_vertexData.getInverseMass(vertices.vertex0);
|
|
float invMass1 = m_vertexData.getInverseMass(vertices.vertex1);
|
|
float linearStiffness = m_linkData.getLinearStiffnessCoefficient(linkIndex);
|
|
float massLSC = (invMass0 + invMass1)/linearStiffness;
|
|
m_linkData.getMassLSC(linkIndex) = massLSC;
|
|
float restLength = m_linkData.getRestLength(linkIndex);
|
|
float restLengthSquared = restLength*restLength;
|
|
m_linkData.getRestLengthSquared(linkIndex) = restLengthSquared;
|
|
}
|
|
}
|
|
} // btCPUSoftBodySolver::updateConstants
|
|
|
|
|
|
|
|
|
|
void btCPUSoftBodySolver::updateBounds()
|
|
{
|
|
using Vectormath::Aos::Point3;
|
|
|
|
for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
|
|
btVector3 startBound(FLT_MAX, FLT_MAX, FLT_MAX);
|
|
btVector3 endBound(FLT_MIN, FLT_MIN, FLT_MIN);
|
|
|
|
const int startVertex = currentCloth->getFirstVertex();
|
|
const int numVertices = currentCloth->getNumVertices();
|
|
|
|
int endVertex = startVertex + numVertices;
|
|
for(int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex)
|
|
{
|
|
btVector3 vertexPosition( m_vertexData.getVertexPositions()[vertexIndex].getX(), m_vertexData.getVertexPositions()[vertexIndex].getY(), m_vertexData.getVertexPositions()[vertexIndex].getZ() );
|
|
startBound.setX( btMin( startBound.getX(), vertexPosition.getX() ) );
|
|
startBound.setY( btMin( startBound.getY(), vertexPosition.getY() ) );
|
|
startBound.setZ( btMin( startBound.getZ(), vertexPosition.getZ() ) );
|
|
|
|
endBound.setX( btMax( endBound.getX(), vertexPosition.getX() ) );
|
|
endBound.setY( btMax( endBound.getY(), vertexPosition.getY() ) );
|
|
endBound.setZ( btMax( endBound.getZ(), vertexPosition.getZ() ) );
|
|
}
|
|
|
|
m_softBodySet[clothIndex]->updateBounds( startBound, endBound );
|
|
}
|
|
}
|
|
|
|
|
|
class btCPUSB_QuickSortCompare
|
|
{
|
|
public:
|
|
|
|
bool operator() ( const btCPUCollisionShapeDescription& a, const btCPUCollisionShapeDescription& b )
|
|
{
|
|
return ( a.softBodyIdentifier < b.softBodyIdentifier );
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Sort the collision object details array and generate indexing into it for the per-cloth collision object array.
|
|
*/
|
|
void btCPUSoftBodySolver::prepareCollisionConstraints()
|
|
{
|
|
// First do a simple sort on the collision objects
|
|
btAlignedObjectArray<int> numObjectsPerClothPrefixSum;
|
|
btAlignedObjectArray<int> numObjectsPerCloth;
|
|
numObjectsPerCloth.resize( m_softBodySet.size(), 0 );
|
|
numObjectsPerClothPrefixSum.resize( m_softBodySet.size(), 0 );
|
|
|
|
if (!m_perClothCollisionObjects.size())
|
|
return;
|
|
|
|
m_collisionObjectDetails.quickSort( btCPUSB_QuickSortCompare() );
|
|
|
|
// Generating indexing for perClothCollisionObjects
|
|
// First clear the previous values with the "no collision object for cloth" constant
|
|
for( int clothIndex = 0; clothIndex < m_perClothCollisionObjects.size(); ++clothIndex )
|
|
{
|
|
m_perClothCollisionObjects[clothIndex].firstObject = -1;
|
|
m_perClothCollisionObjects[clothIndex].endObject = -1;
|
|
}
|
|
int currentCloth = 0;
|
|
int startIndex = 0;
|
|
for( int collisionObject = 0; collisionObject < m_collisionObjectDetails.size(); ++collisionObject )
|
|
{
|
|
int nextCloth = m_collisionObjectDetails[collisionObject].softBodyIdentifier;
|
|
if( nextCloth != currentCloth )
|
|
{
|
|
// Changed cloth in the array
|
|
// Set the end index and the range is what we need for currentCloth
|
|
m_perClothCollisionObjects[currentCloth].firstObject = startIndex;
|
|
m_perClothCollisionObjects[currentCloth].endObject = collisionObject;
|
|
currentCloth = nextCloth;
|
|
startIndex = collisionObject;
|
|
}
|
|
}
|
|
|
|
// And update last cloth
|
|
m_perClothCollisionObjects[currentCloth].firstObject = startIndex;
|
|
m_perClothCollisionObjects[currentCloth].endObject = m_collisionObjectDetails.size();
|
|
|
|
} // prepareCollisionConstraints
|
|
|
|
|
|
void btCPUSoftBodySolver::solveConstraints( float solverdt )
|
|
{
|
|
using Vectormath::Aos::Vector3;
|
|
using Vectormath::Aos::Point3;
|
|
using Vectormath::Aos::lengthSqr;
|
|
using Vectormath::Aos::dot;
|
|
|
|
// Prepare links
|
|
int numLinks = m_linkData.getNumLinks();
|
|
int numVertices = m_vertexData.getNumVertices();
|
|
|
|
float kst = 1.f;
|
|
|
|
for( int linkIndex = 0; linkIndex < numLinks; ++linkIndex )
|
|
{
|
|
btSoftBodyLinkData::LinkNodePair &nodePair( m_linkData.getVertexPair(linkIndex) );
|
|
Vector3 currentLength = m_vertexData.getPreviousPosition( nodePair.vertex1 ) - m_vertexData.getPreviousPosition( nodePair.vertex0 );
|
|
m_linkData.getCurrentLength(linkIndex) = currentLength;
|
|
|
|
// If mass at both ends of links is 0 (both static points) then we don't want this information.
|
|
// In reality this would be a fairly pointless link, but it could have been inserted
|
|
float linkLengthRatio = 0;
|
|
if( m_linkData.getMassLSC(linkIndex) > 0 )
|
|
linkLengthRatio = 1.f/(lengthSqr(currentLength) * m_linkData.getMassLSC(linkIndex));
|
|
m_linkData.getLinkLengthRatio(linkIndex) = linkLengthRatio;
|
|
|
|
}
|
|
|
|
|
|
prepareCollisionConstraints();
|
|
|
|
|
|
for( int iteration = 0; iteration < m_numberOfVelocityIterations ; ++iteration )
|
|
{
|
|
// Solve velocity
|
|
for(int linkIndex = 0; linkIndex < numLinks; ++linkIndex)
|
|
{
|
|
|
|
int vertexIndex0 = m_linkData.getVertexPair(linkIndex).vertex0;
|
|
int vertexIndex1 = m_linkData.getVertexPair(linkIndex).vertex1;
|
|
|
|
float j = -dot(m_linkData.getCurrentLength(linkIndex), m_vertexData.getVelocity(vertexIndex0) - m_vertexData.getVelocity(vertexIndex1)) * m_linkData.getLinkLengthRatio(linkIndex)*kst;
|
|
|
|
// If both ends of the link have no mass then this will be zero. Catch that case.
|
|
// TODO: Should really catch the /0 in the link setup, too
|
|
//if(psb->m_linksc0[i]>0)
|
|
{
|
|
m_vertexData.getVelocity(vertexIndex0) = m_vertexData.getVelocity(vertexIndex0) + m_linkData.getCurrentLength(linkIndex)*j*m_vertexData.getInverseMass(vertexIndex0);
|
|
m_vertexData.getVelocity(vertexIndex1) = m_vertexData.getVelocity(vertexIndex1) - m_linkData.getCurrentLength(linkIndex)*j*m_vertexData.getInverseMass(vertexIndex1);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Compute new positions from velocity
|
|
// Also update the previous position so that our position computation is now based on the new position from the velocity solution
|
|
// rather than based directly on the original positions
|
|
if( m_numberOfVelocityIterations > 0 )
|
|
{
|
|
for(int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex)
|
|
{
|
|
m_vertexData.getPosition(vertexIndex) = m_vertexData.getPreviousPosition(vertexIndex) + m_vertexData.getVelocity(vertexIndex) * solverdt;
|
|
m_vertexData.getPreviousPosition(vertexIndex) = m_vertexData.getPosition(vertexIndex);
|
|
}
|
|
}
|
|
|
|
// Solve drift
|
|
for( int iteration = 0; iteration < m_numberOfPositionIterations ; ++iteration )
|
|
{
|
|
for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
|
|
|
|
const int startLink = currentCloth->getFirstLink();
|
|
const int numLinks = currentCloth->getNumLinks();
|
|
|
|
int endLink = startLink + numLinks;
|
|
for(int linkIndex = startLink; linkIndex < endLink; ++linkIndex)
|
|
{
|
|
int vertexIndex0 = m_linkData.getVertexPair(linkIndex).vertex0;
|
|
int vertexIndex1 = m_linkData.getVertexPair(linkIndex).vertex1;
|
|
|
|
float massLSC = m_linkData.getMassLSC(linkIndex);
|
|
if( massLSC > 0.f )
|
|
{
|
|
Point3 &vertexPosition0( m_vertexData.getPosition( vertexIndex0 ) );
|
|
Point3 &vertexPosition1( m_vertexData.getPosition( vertexIndex1 ) );
|
|
|
|
Vector3 del = vertexPosition1 - vertexPosition0;
|
|
float len = lengthSqr(del);
|
|
float restLength2 = m_linkData.getRestLengthSquared(linkIndex);
|
|
float k = ((restLength2 - len) / (massLSC * (restLength2 + len) ) )*kst;
|
|
|
|
vertexPosition0 -= del*(k*m_vertexData.getInverseMass(vertexIndex0));
|
|
vertexPosition1 += del*(k*m_vertexData.getInverseMass(vertexIndex1));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Clear forces so that friction is applied correctly
|
|
for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
|
|
|
|
const int startLink = currentCloth->getFirstLink();
|
|
const int numLinks = currentCloth->getNumLinks();
|
|
const int startVertex = currentCloth->getFirstVertex();
|
|
const int numVertices = currentCloth->getNumVertices();
|
|
const int lastVertex = startVertex + numVertices;
|
|
// Update the velocities based on the change in position
|
|
// TODO: Damping should only be applied to the action of link constraints so the cloth still falls but then moves stiffly once it hits something
|
|
float velocityCoefficient = (1.f - m_perClothDampingFactor[clothIndex]);
|
|
float velocityCorrectionCoefficient = m_perClothVelocityCorrectionCoefficient[clothIndex];
|
|
float isolverDt = 1.f/solverdt;
|
|
|
|
for(int vertexIndex = startVertex; vertexIndex < lastVertex; ++vertexIndex)
|
|
{
|
|
m_vertexData.getForceAccumulator( vertexIndex ) = Vector3(0.f, 0.f, 0.f);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
// Solve collision constraints
|
|
// Very simple solver that pushes the vertex out of collision imposters for now
|
|
// to test integration with the broad phase code.
|
|
// May also want to put this into position solver loop every n iterations depending on
|
|
// how it behaves
|
|
for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
|
|
|
|
float clothFriction = currentCloth->getSoftBody()->getFriction();
|
|
|
|
const int startVertex = currentCloth->getFirstVertex();
|
|
const int numVertices = currentCloth->getNumVertices();
|
|
int endVertex = startVertex + numVertices;
|
|
|
|
float velocityCoefficient = (1.f - m_perClothDampingFactor[clothIndex]);
|
|
float velocityCorrectionCoefficient = m_perClothVelocityCorrectionCoefficient[clothIndex];
|
|
float isolverDt = 1.f/solverdt;
|
|
|
|
int startObject = m_perClothCollisionObjects[clothIndex].firstObject;
|
|
int endObject = m_perClothCollisionObjects[clothIndex].endObject;
|
|
|
|
if( endObject == startObject )
|
|
{
|
|
// No collisions so just do the force update
|
|
for(int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex)
|
|
{
|
|
m_vertexData.getForceAccumulator( vertexIndex ) = Vector3(0.f, 0.f, 0.f);
|
|
}
|
|
|
|
// Recompute velocity based on updated position inclusive of drift
|
|
for(int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex)
|
|
{
|
|
m_vertexData.getVelocity(vertexIndex) = (m_vertexData.getPosition(vertexIndex) - m_vertexData.getPreviousPosition(vertexIndex)) * velocityCoefficient * isolverDt;
|
|
}
|
|
} else {
|
|
|
|
for( int collisionObject = startObject; collisionObject < endObject; ++collisionObject )
|
|
{
|
|
btCPUCollisionShapeDescription &shapeDescription( m_collisionObjectDetails[collisionObject] );
|
|
|
|
float colliderFriction = shapeDescription.friction;
|
|
|
|
if( shapeDescription.collisionShapeType == CAPSULE_SHAPE_PROXYTYPE )
|
|
{
|
|
using namespace Vectormath::Aos;
|
|
|
|
float capsuleHalfHeight = shapeDescription.shapeInformation.capsule.halfHeight;
|
|
float capsuleRadius = shapeDescription.shapeInformation.capsule.radius;
|
|
int capsuleUpAxis = shapeDescription.shapeInformation.capsule.upAxis;
|
|
float capsuleMargin = shapeDescription.margin;
|
|
|
|
Transform3 worldTransform = shapeDescription.shapeTransform;
|
|
|
|
// As this is a GPU comparison solver just iterate over the vertices
|
|
for( int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex )
|
|
{
|
|
// Clear force for vertex first
|
|
m_vertexData.getForceAccumulator( vertexIndex ) = Vector3(0.f, 0.f, 0.f);
|
|
|
|
Point3 vertex( m_vertexData.getPosition( vertexIndex ) );
|
|
|
|
// Correctly define the centerline depending on the upAxis
|
|
Point3 c1(0.f, 0.f, 0.f);
|
|
Point3 c2(0.f, 0.f, 0.f);
|
|
if( capsuleUpAxis == 0 ) {
|
|
c1.setX(-capsuleHalfHeight);
|
|
c2.setX(capsuleHalfHeight);
|
|
} else if( capsuleUpAxis == 1 ) {
|
|
c1.setY(-capsuleHalfHeight);
|
|
c2.setY(capsuleHalfHeight);
|
|
} else {
|
|
c1.setZ(-capsuleHalfHeight);
|
|
c2.setZ(capsuleHalfHeight);
|
|
}
|
|
|
|
Point3 worldC1 = worldTransform * c1;
|
|
Point3 worldC2 = worldTransform * c2;
|
|
Vector3 segment = worldC2 - worldC1;
|
|
|
|
// compute distance of tangent to vertex along line segment in capsule
|
|
float distanceAlongSegment = -( dot( worldC1 - vertex, segment ) / lengthSqr(segment) );
|
|
|
|
Point3 closestPoint = (worldC1 + segment * distanceAlongSegment);
|
|
float distanceFromLine = length(vertex - closestPoint);
|
|
float distanceFromC1 = length(worldC1 - vertex);
|
|
float distanceFromC2 = length(worldC2 - vertex);
|
|
|
|
// Final distance from collision, point to push from, direction to push in
|
|
// for impulse force
|
|
float distance;
|
|
Point3 sourcePoint;
|
|
Vector3 normalVector;
|
|
if( distanceAlongSegment < 0 )
|
|
{
|
|
distance = distanceFromC1;
|
|
sourcePoint = worldC1;
|
|
normalVector = normalize(vertex - worldC1);
|
|
} else if( distanceAlongSegment > 1.f ) {
|
|
distance = distanceFromC2;
|
|
sourcePoint = worldC2;
|
|
normalVector = normalize(vertex - worldC2);
|
|
} else {
|
|
distance = distanceFromLine;
|
|
sourcePoint = closestPoint;
|
|
normalVector = normalize(vertex - closestPoint);
|
|
}
|
|
|
|
Vector3 colliderLinearVelocity( shapeDescription.linearVelocity );
|
|
Vector3 colliderAngularVelocity( shapeDescription.angularVelocity );
|
|
Vector3 velocityOfSurfacePoint = colliderLinearVelocity + cross(colliderAngularVelocity, Vector3(vertex) - worldTransform.getTranslation());
|
|
|
|
float minDistance = capsuleRadius + capsuleMargin;
|
|
bool collided = false;
|
|
|
|
if( distance < minDistance )
|
|
{
|
|
// Project back to surface along normal
|
|
Vectormath::Aos::Point3 sourcePos = m_vertexData.getPosition( vertexIndex );
|
|
Vectormath::Aos::Vector3 posChange = (minDistance - distance)*normalVector*0.9;
|
|
//if( length(posChange) > 1 )
|
|
// std::cerr << "Poschange: " << length(posChange) << "\n";
|
|
|
|
Vectormath::Aos::Point3 newPos = sourcePos + posChange;
|
|
m_vertexData.getPosition( vertexIndex ) = newPos;
|
|
//m_vertexData.getPosition( vertexIndex ) = m_vertexData.getPosition( vertexIndex ) + (minDistance - distance)*normalVector*0.9;
|
|
|
|
// Experiment with moving back along source vector.
|
|
// Removes all ability to slide because it projects back along the vector length so it would undo lateral movement.
|
|
// TODO: This isn't quite right because we should take the movement of the collider into account as well
|
|
/*Vector3 incomingMoveVector( normalize(m_vertexData.getPreviousPosition(vertexIndex) - m_vertexData.getPosition(vertexIndex)) );
|
|
Vector3 normalDirectionMoveOut( (minDistance - distance)*normalVector*0.9 );
|
|
float distanceOnIncomingVector = dot(normalDirectionMoveOut, incomingMoveVector);
|
|
Vector3 vectorCorrection( distanceOnIncomingVector*incomingMoveVector );
|
|
m_vertexData.getPosition( vertexIndex ) = m_vertexData.getPosition( vertexIndex ) + vectorCorrection;*/
|
|
|
|
|
|
collided = true;
|
|
}
|
|
|
|
// Update velocity of vertex based on position
|
|
m_vertexData.getVelocity(vertexIndex) = (m_vertexData.getPosition(vertexIndex) - m_vertexData.getPreviousPosition(vertexIndex)) * velocityCoefficient * isolverDt;
|
|
|
|
// If we collided before we are on the surface so have friction
|
|
if( collided )
|
|
{
|
|
// Compute friction
|
|
|
|
// TODO: Just vertex velocity not enough, really we need the velocity
|
|
// relative to closest point on the surface of the collider
|
|
Vector3 vertexVelocity( m_vertexData.getVelocity(vertexIndex) );
|
|
Vector3 relativeVelocity( vertexVelocity - velocityOfSurfacePoint );
|
|
|
|
|
|
// First compute vectors for plane perpendicular to normal vector
|
|
// Cross any vector with normal vector first then cross the normal with it again
|
|
Vector3 p1(normalize(cross(normalVector, segment)));
|
|
Vector3 p2(normalize(cross(p1, normalVector)));
|
|
// Full friction is sum of velocities in each direction of plane.
|
|
Vector3 frictionVector(p1*dot(relativeVelocity, p1) + p2*dot(relativeVelocity, p2));
|
|
|
|
// Real friction is peak friction corrected by friction coefficients.
|
|
frictionVector = frictionVector*(colliderFriction*clothFriction);
|
|
|
|
float approachSpeed = dot( relativeVelocity, normalVector );
|
|
|
|
// For now just update vertex position by moving to radius distance along the push vector
|
|
// Could use this as the basis for simple vector distance constraint for the point later, possibly?
|
|
// That way in the main solver loop all shape types could be the same... though when
|
|
// we need to apply bi-directionally it becomes more complicated
|
|
|
|
// Add friction vector to the force accumulator
|
|
Vector3 ¤tForce( m_vertexData.getForceAccumulator( vertexIndex ) );
|
|
|
|
// Only apply if the vertex is moving towards the object to reduce jitter error
|
|
if( approachSpeed <= 0.0 )
|
|
currentForce -= frictionVector;
|
|
}
|
|
}
|
|
}
|
|
} // for( int collisionObject = startObject; collisionObject < endObject; ++collisionObject )
|
|
} // if( endObject == startObject )
|
|
}
|
|
|
|
|
|
|
|
|
|
} // btCPUSoftBodySolver::solveConstraints
|
|
|
|
|
|
btCPUSoftBodySolver::btAcceleratedSoftBodyInterface *btCPUSoftBodySolver::findSoftBodyInterface( const btSoftBody* const softBody )
|
|
{
|
|
for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *softBodyInterface = m_softBodySet[softBodyIndex];
|
|
if( softBodyInterface->getSoftBody() == softBody )
|
|
return softBodyInterface;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
const btCPUSoftBodySolver::btAcceleratedSoftBodyInterface * const btCPUSoftBodySolver::findSoftBodyInterface( const btSoftBody* const softBody ) const
|
|
{
|
|
for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
|
|
{
|
|
const btAcceleratedSoftBodyInterface *const softBodyInterface = m_softBodySet[softBodyIndex];
|
|
if( softBodyInterface->getSoftBody() == softBody )
|
|
return softBodyInterface;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int btCPUSoftBodySolver::findSoftBodyIndex( const btSoftBody* const softBody )
|
|
{
|
|
for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
|
|
{
|
|
btAcceleratedSoftBodyInterface *softBodyInterface = m_softBodySet[softBodyIndex];
|
|
if( softBodyInterface->getSoftBody() == softBody )
|
|
return softBodyIndex;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
void btSoftBodySolverOutputCPUtoCPU::copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer )
|
|
{
|
|
// Currently only support CPU output buffers
|
|
|
|
const btSoftBodySolver *solver = softBody->getSoftBodySolver();
|
|
btAssert( solver->getSolverType() == btSoftBodySolver::CPU_SOLVER );
|
|
const btCPUSoftBodySolver *cpuSolver = static_cast< const btCPUSoftBodySolver * >( solver );
|
|
const btCPUSoftBodySolver::btAcceleratedSoftBodyInterface * const currentCloth = cpuSolver->findSoftBodyInterface( softBody );
|
|
const btSoftBodyVertexData &vertexData( cpuSolver->m_vertexData );
|
|
|
|
if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER )
|
|
{
|
|
const int firstVertex = currentCloth->getFirstVertex();
|
|
const int lastVertex = firstVertex + currentCloth->getNumVertices();
|
|
const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer);
|
|
float *basePointer = cpuVertexBuffer->getBasePointer();
|
|
|
|
if( vertexBuffer->hasVertexPositions() )
|
|
{
|
|
const int vertexOffset = cpuVertexBuffer->getVertexOffset();
|
|
const int vertexStride = cpuVertexBuffer->getVertexStride();
|
|
float *vertexPointer = basePointer + vertexOffset;
|
|
|
|
for( int vertexIndex = firstVertex; vertexIndex < lastVertex; ++vertexIndex )
|
|
{
|
|
Vectormath::Aos::Point3 position = vertexData.getPosition(vertexIndex);
|
|
*(vertexPointer + 0) = position.getX();
|
|
*(vertexPointer + 1) = position.getY();
|
|
*(vertexPointer + 2) = position.getZ();
|
|
vertexPointer += vertexStride;
|
|
}
|
|
}
|
|
if( vertexBuffer->hasNormals() )
|
|
{
|
|
const int normalOffset = cpuVertexBuffer->getNormalOffset();
|
|
const int normalStride = cpuVertexBuffer->getNormalStride();
|
|
float *normalPointer = basePointer + normalOffset;
|
|
|
|
for( int vertexIndex = firstVertex; vertexIndex < lastVertex; ++vertexIndex )
|
|
{
|
|
Vectormath::Aos::Vector3 normal = vertexData.getNormal(vertexIndex);
|
|
*(normalPointer + 0) = normal.getX();
|
|
*(normalPointer + 1) = normal.getY();
|
|
*(normalPointer + 2) = normal.getZ();
|
|
normalPointer += normalStride;
|
|
}
|
|
}
|
|
} else {
|
|
btAssert( 0=="Invalid vertex buffer descriptor used in CPU output." );
|
|
}
|
|
} // btCPUSoftBodySolver::outputToVertexBuffers
|
|
|
|
void btCPUSoftBodySolver::processCollision( btSoftBody*, btSoftBody *)
|
|
{
|
|
|
|
}
|
|
|
|
// Add the collision object to the set to deal with for a particular soft body
|
|
void btCPUSoftBodySolver::processCollision( btSoftBody *softBody, btCollisionObject* collisionObject )
|
|
{
|
|
int softBodyIndex = findSoftBodyIndex( softBody );
|
|
|
|
if( softBodyIndex >= 0 )
|
|
{
|
|
btCollisionShape *collisionShape = collisionObject->getCollisionShape();
|
|
float friction = collisionObject->getFriction();
|
|
int shapeType = collisionShape->getShapeType();
|
|
if( shapeType == CAPSULE_SHAPE_PROXYTYPE )
|
|
{
|
|
// Add to the list of expected collision objects
|
|
btCPUCollisionShapeDescription newCollisionShapeDescription;
|
|
newCollisionShapeDescription.softBodyIdentifier = softBodyIndex;
|
|
newCollisionShapeDescription.collisionShapeType = shapeType;
|
|
newCollisionShapeDescription.shapeTransform = toTransform3(collisionObject->getWorldTransform());
|
|
btCapsuleShape *capsule = static_cast<btCapsuleShape*>( collisionShape );
|
|
newCollisionShapeDescription.shapeInformation.capsule.radius = capsule->getRadius();
|
|
newCollisionShapeDescription.shapeInformation.capsule.halfHeight = capsule->getHalfHeight();
|
|
newCollisionShapeDescription.shapeInformation.capsule.upAxis = capsule->getUpAxis();
|
|
newCollisionShapeDescription.margin = capsule->getMargin();
|
|
newCollisionShapeDescription.friction = friction;
|
|
btRigidBody* body = static_cast< btRigidBody* >( collisionObject );
|
|
newCollisionShapeDescription.linearVelocity = toVector3(body->getLinearVelocity());
|
|
newCollisionShapeDescription.angularVelocity = toVector3(body->getAngularVelocity());
|
|
m_collisionObjectDetails.push_back( newCollisionShapeDescription );
|
|
} else {
|
|
btAssert("Unsupported collision shape type\n");
|
|
}
|
|
} else {
|
|
btAssert("Unknown soft body");
|
|
}
|
|
} // btCPUSoftBodySolver::processCollision
|
|
|
|
|
|
void btCPUSoftBodySolver::predictMotion( float timeStep )
|
|
{
|
|
// Fill the force arrays with current acceleration data etc
|
|
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());
|
|
}
|
|
|
|
|
|
// Apply forces that we know about to the cloths
|
|
applyForces( timeStep * getTimeScale() );
|
|
|
|
// Itegrate motion for all soft bodies dealt with by the solver
|
|
integrate( timeStep * getTimeScale() );
|
|
|
|
// Update bounds
|
|
// Will update the bounds for all softBodies being dealt with by the solver and
|
|
// set the values in the btSoftBody object
|
|
updateBounds();
|
|
|
|
// End prediction work for solvers
|
|
}
|
|
|
|
|