1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
17 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
18 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
19 #include "vectormath/vmInclude.h"
20
21 #include "BulletMultiThreaded/GpuSoftBodySolvers/CPU/btSoftBodySolver_CPU.h"
22 #include "BulletSoftBody/btSoftBody.h"
23 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
24
25
btCPUSoftBodySolver()26 btCPUSoftBodySolver::btCPUSoftBodySolver()
27 {
28 // Initial we will clearly need to update solver constants
29 // For now this is global for the cloths linked with this solver - we should probably make this body specific
30 // for performance in future once we understand more clearly when constants need to be updated
31 m_updateSolverConstants = true;
32 }
33
~btCPUSoftBodySolver()34 btCPUSoftBodySolver::~btCPUSoftBodySolver()
35 {
36 }
37
38
39
40
getLinkData()41 btSoftBodyLinkData &btCPUSoftBodySolver::getLinkData()
42 {
43 return m_linkData;
44 }
45
getVertexData()46 btSoftBodyVertexData &btCPUSoftBodySolver::getVertexData()
47 {
48 return m_vertexData;
49 }
50
getTriangleData()51 btSoftBodyTriangleData &btCPUSoftBodySolver::getTriangleData()
52 {
53 return m_triangleData;
54 }
55
56
57
58
59
60
toVector3(const btVector3 & vec)61 static Vectormath::Aos::Vector3 toVector3( const btVector3 &vec )
62 {
63 Vectormath::Aos::Vector3 outVec( vec.getX(), vec.getY(), vec.getZ() );
64 return outVec;
65 }
66
toTransform3(const btTransform & transform)67 static Vectormath::Aos::Transform3 toTransform3( const btTransform &transform )
68 {
69 Vectormath::Aos::Transform3 outTransform;
70 outTransform.setCol(0, toVector3(transform.getBasis().getColumn(0)));
71 outTransform.setCol(1, toVector3(transform.getBasis().getColumn(1)));
72 outTransform.setCol(2, toVector3(transform.getBasis().getColumn(2)));
73 outTransform.setCol(3, toVector3(transform.getOrigin()));
74 return outTransform;
75 }
76
updateBounds(const btVector3 & lowerBound,const btVector3 & upperBound)77 void btCPUSoftBodySolver::btAcceleratedSoftBodyInterface::updateBounds( const btVector3 &lowerBound, const btVector3 &upperBound )
78 {
79 float scalarMargin = this->getSoftBody()->getCollisionShape()->getMargin();
80 btVector3 vectorMargin( scalarMargin, scalarMargin, scalarMargin );
81 m_softBody->m_bounds[0] = lowerBound - vectorMargin;
82 m_softBody->m_bounds[1] = upperBound + vectorMargin;
83 }
84
85
copyBackToSoftBodies()86 void btCPUSoftBodySolver::copyBackToSoftBodies()
87 {
88 // Loop over soft bodies, copying all the vertex positions back for each body in turn
89 for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
90 {
91 btAcceleratedSoftBodyInterface *softBodyInterface = m_softBodySet[ softBodyIndex ];
92 btSoftBody *softBody = softBodyInterface->getSoftBody();
93
94 int firstVertex = softBodyInterface->getFirstVertex();
95 int numVertices = softBodyInterface->getNumVertices();
96
97 // Copy vertices from solver back into the softbody
98 for( int vertex = 0; vertex < numVertices; ++vertex )
99 {
100 using Vectormath::Aos::Point3;
101 Point3 vertexPosition( getVertexData().getVertexPositions()[firstVertex + vertex] );
102
103 softBody->m_nodes[vertex].m_x.setX( vertexPosition.getX() );
104 softBody->m_nodes[vertex].m_x.setY( vertexPosition.getY() );
105 softBody->m_nodes[vertex].m_x.setZ( vertexPosition.getZ() );
106
107 softBody->m_nodes[vertex].m_n.setX( vertexPosition.getX() );
108 softBody->m_nodes[vertex].m_n.setY( vertexPosition.getY() );
109 softBody->m_nodes[vertex].m_n.setZ( vertexPosition.getZ() );
110 }
111 }
112 } // btCPUSoftBodySolver::copyBackToSoftBodies
113
optimize(btAlignedObjectArray<btSoftBody * > & softBodies,bool forceUpdate)114 void btCPUSoftBodySolver::optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate )
115 {
116 if( forceUpdate || m_softBodySet.size() != softBodies.size() )
117 {
118 // Have a change in the soft body set so update, reloading all the data
119 getVertexData().clear();
120 getTriangleData().clear();
121 getLinkData().clear();
122 m_softBodySet.resize(0);
123
124
125 for( int softBodyIndex = 0; softBodyIndex < softBodies.size(); ++softBodyIndex )
126 {
127 btSoftBody *softBody = softBodies[ softBodyIndex ];
128 using Vectormath::Aos::Matrix3;
129 using Vectormath::Aos::Point3;
130
131 // Create SoftBody that will store the information within the solver
132 btAcceleratedSoftBodyInterface *newSoftBody = new btAcceleratedSoftBodyInterface( softBody );
133 m_softBodySet.push_back( newSoftBody );
134
135 m_perClothAcceleration.push_back( toVector3(softBody->getWorldInfo()->m_gravity) );
136 m_perClothDampingFactor.push_back(softBody->m_cfg.kDP);
137 m_perClothVelocityCorrectionCoefficient.push_back( softBody->m_cfg.kVCF );
138 m_perClothLiftFactor.push_back( softBody->m_cfg.kLF );
139 m_perClothDragFactor.push_back( softBody->m_cfg.kDG );
140 m_perClothMediumDensity.push_back(softBody->getWorldInfo()->air_density);
141 m_perClothCollisionObjects.push_back( CollisionObjectIndices(-1, -1) );
142
143 // Add space for new vertices and triangles in the default solver for now
144 // TODO: Include space here for tearing too later
145 int firstVertex = getVertexData().getNumVertices();
146 int numVertices = softBody->m_nodes.size();
147 int maxVertices = numVertices;
148 // Allocate space for new vertices in all the vertex arrays
149 getVertexData().createVertices( maxVertices, softBodyIndex );
150
151 int firstTriangle = getTriangleData().getNumTriangles();
152 int numTriangles = softBody->m_faces.size();
153 int maxTriangles = numTriangles;
154 getTriangleData().createTriangles( maxTriangles );
155
156 // Copy vertices from softbody into the solver
157 for( int vertex = 0; vertex < numVertices; ++vertex )
158 {
159 Point3 multPoint(softBody->m_nodes[vertex].m_x.getX(), softBody->m_nodes[vertex].m_x.getY(), softBody->m_nodes[vertex].m_x.getZ());
160 btSoftBodyVertexData::VertexDescription desc;
161
162 // TODO: Position in the softbody might be pre-transformed
163 // or we may need to adapt for the pose.
164 //desc.setPosition( cloth.getMeshTransform()*multPoint );
165 desc.setPosition( multPoint );
166
167 float vertexInverseMass = softBody->m_nodes[vertex].m_im;
168 desc.setInverseMass(vertexInverseMass);
169 getVertexData().setVertexAt( desc, firstVertex + vertex );
170 }
171
172 // Copy triangles similarly
173 // We're assuming here that vertex indices are based on the firstVertex rather than the entire scene
174 for( int triangle = 0; triangle < numTriangles; ++triangle )
175 {
176 // Note that large array storage is relative to the array not to the cloth
177 // So we need to add firstVertex to each value
178 int vertexIndex0 = (softBody->m_faces[triangle].m_n[0] - &(softBody->m_nodes[0]));
179 int vertexIndex1 = (softBody->m_faces[triangle].m_n[1] - &(softBody->m_nodes[0]));
180 int vertexIndex2 = (softBody->m_faces[triangle].m_n[2] - &(softBody->m_nodes[0]));
181 btSoftBodyTriangleData::TriangleDescription newTriangle(vertexIndex0 + firstVertex, vertexIndex1 + firstVertex, vertexIndex2 + firstVertex);
182 getTriangleData().setTriangleAt( newTriangle, firstTriangle + triangle );
183
184 // Increase vertex triangle counts for this triangle
185 getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex0)++;
186 getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex1)++;
187 getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex2)++;
188 }
189
190 int firstLink = getLinkData().getNumLinks();
191 int numLinks = softBody->m_links.size();
192 int maxLinks = numLinks;
193
194 // Allocate space for the links
195 getLinkData().createLinks( numLinks );
196
197 // Add the links
198 for( int link = 0; link < numLinks; ++link )
199 {
200 int vertexIndex0 = softBody->m_links[link].m_n[0] - &(softBody->m_nodes[0]);
201 int vertexIndex1 = softBody->m_links[link].m_n[1] - &(softBody->m_nodes[0]);
202
203 btSoftBodyLinkData::LinkDescription newLink(vertexIndex0 + firstVertex, vertexIndex1 + firstVertex, softBody->m_links[link].m_material->m_kLST);
204 newLink.setLinkStrength(1.f);
205 getLinkData().setLinkAt(newLink, firstLink + link);
206 }
207
208 newSoftBody->setFirstVertex( firstVertex );
209 newSoftBody->setFirstTriangle( firstTriangle );
210 newSoftBody->setNumVertices( numVertices );
211 newSoftBody->setMaxVertices( maxVertices );
212 newSoftBody->setNumTriangles( numTriangles );
213 newSoftBody->setMaxTriangles( maxTriangles );
214 newSoftBody->setFirstLink( firstLink );
215 newSoftBody->setNumLinks( numLinks );
216 }
217
218
219
220 updateConstants(0.f);
221 }
222 }
223
224
225
226
updateSoftBodies()227 void btCPUSoftBodySolver::updateSoftBodies()
228 {
229 using namespace Vectormath::Aos;
230
231 int numVertices = m_vertexData.getNumVertices();
232 int numTriangles = m_triangleData.getNumTriangles();
233
234 // Initialise normal and vertex counts
235 for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
236 {
237 m_vertexData.getArea(vertexIndex) = 0.f;
238 m_vertexData.getNormal(vertexIndex) = Vector3(0.f, 0.f, 0.f);
239 }
240
241 // Update the areas for the triangles and vertices.
242 for( int triangleIndex = 0; triangleIndex < numTriangles; ++triangleIndex )
243 {
244 float &triangleArea( m_triangleData.getTriangleArea( triangleIndex ) );
245 const btSoftBodyTriangleData::TriangleNodeSet &vertices( m_triangleData.getVertexSet(triangleIndex) );
246
247 Point3 &vertexPosition0( m_vertexData.getPosition( vertices.vertex0 ) );
248 Point3 &vertexPosition1( m_vertexData.getPosition( vertices.vertex1 ) );
249 Point3 &vertexPosition2( m_vertexData.getPosition( vertices.vertex2 ) );
250
251 triangleArea = computeTriangleArea( vertexPosition0, vertexPosition1, vertexPosition2 );
252
253 // Add to areas for vertices and increase the count of the number of triangles affecting the vertex
254 m_vertexData.getArea(vertices.vertex0) += triangleArea;
255 m_vertexData.getArea(vertices.vertex1) += triangleArea;
256 m_vertexData.getArea(vertices.vertex2) += triangleArea;
257
258 Point3 &vertex0( m_vertexData.getPosition(vertices.vertex0) );
259 Point3 &vertex1( m_vertexData.getPosition(vertices.vertex1) );
260 Point3 &vertex2( m_vertexData.getPosition(vertices.vertex2) );
261
262 Vector3 triangleNormal = cross( vertex1-vertex0, vertex2 - vertex0 );
263
264 m_triangleData.getNormal(triangleIndex) = normalize(triangleNormal);
265
266 m_vertexData.getNormal(vertices.vertex0) += triangleNormal;
267 m_vertexData.getNormal(vertices.vertex1) += triangleNormal;
268 m_vertexData.getNormal(vertices.vertex2) += triangleNormal;
269
270 }
271
272 // Normalise the area and normals
273 for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
274 {
275 m_vertexData.getArea(vertexIndex) /= m_vertexData.getTriangleCount(vertexIndex);
276 m_vertexData.getNormal(vertexIndex) = normalize( m_vertexData.getNormal(vertexIndex) );
277 }
278
279
280 // Clear the collision shape array for the next frame
281 m_collisionObjectDetails.clear();
282
283 } // updateSoftBodies
284
285
ProjectOnAxis(const Vectormath::Aos::Vector3 & v,const Vectormath::Aos::Vector3 & a)286 Vectormath::Aos::Vector3 btCPUSoftBodySolver::ProjectOnAxis( const Vectormath::Aos::Vector3 &v, const Vectormath::Aos::Vector3 &a )
287 {
288 return a*Vectormath::Aos::dot(v, a);
289 }
290
ApplyClampedForce(float solverdt,const Vectormath::Aos::Vector3 & force,const Vectormath::Aos::Vector3 & vertexVelocity,float inverseMass,Vectormath::Aos::Vector3 & vertexForce)291 void btCPUSoftBodySolver::ApplyClampedForce( float solverdt, const Vectormath::Aos::Vector3 &force, const Vectormath::Aos::Vector3 &vertexVelocity, float inverseMass, Vectormath::Aos::Vector3 &vertexForce )
292 {
293 float dtInverseMass = solverdt*inverseMass;
294 if( Vectormath::Aos::lengthSqr(force * dtInverseMass) > Vectormath::Aos::lengthSqr(vertexVelocity) )
295 {
296 vertexForce -= ProjectOnAxis( vertexVelocity, normalize( force ) )/dtInverseMass;
297 } else {
298 vertexForce += force;
299 }
300 }
301
checkInitialized()302 bool btCPUSoftBodySolver::checkInitialized()
303 {
304 return true;
305 }
306
applyForces(float solverdt)307 void btCPUSoftBodySolver::applyForces( float solverdt )
308 {
309 using namespace Vectormath::Aos;
310
311 int numVertices = m_vertexData.getNumVertices();
312 for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
313 {
314 btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
315 const int startVertex = currentCloth->getFirstVertex();
316 const int numVertices = currentCloth->getNumVertices();
317
318 Vector3 velocityChange = m_perClothAcceleration[clothIndex]*solverdt;
319 for( int vertexIndex = startVertex; vertexIndex < (startVertex + numVertices); ++vertexIndex )
320 {
321 float inverseMass = m_vertexData.getInverseMass( vertexIndex );
322 Vector3 &vertexVelocity( m_vertexData.getVelocity( vertexIndex ) );
323
324 // First apply the global acceleration to all vertices
325 if( inverseMass > 0 )
326 vertexVelocity += velocityChange;
327
328 // If it's a non-static vertex
329 if( m_vertexData.getInverseMass(vertexIndex) > 0 )
330 {
331 // Wind effects on a wind-per-cloth basis
332 float liftFactor = m_perClothLiftFactor[clothIndex];
333 float dragFactor = m_perClothDragFactor[clothIndex];
334 if( (liftFactor > 0.f) || (dragFactor > 0.f) )
335 {
336 Vector3 normal = m_vertexData.getNormal(vertexIndex);
337 Vector3 relativeWindVelocity = m_vertexData.getVelocity(vertexIndex) - m_perClothWindVelocity[clothIndex];
338 float relativeSpeedSquared = lengthSqr(relativeWindVelocity);
339 if( relativeSpeedSquared > FLT_EPSILON )
340 {
341 normal = normal * (dot(normal, relativeWindVelocity) < 0 ? -1.f : +1.f);
342 float dvNormal = dot(normal, relativeWindVelocity);
343 if( dvNormal > 0 )
344 {
345 Vector3 force( 0.f, 0.f, 0.f );
346 float c0 = m_vertexData.getArea(vertexIndex) * dvNormal * relativeSpeedSquared / 2;
347 float c1 = c0 * m_perClothMediumDensity[clothIndex];
348 force += normal * (-c1 * liftFactor);
349 force += normalize(relativeWindVelocity)*(-c1 * dragFactor);
350
351 Vectormath::Aos::Vector3 &vertexForce( m_vertexData.getForceAccumulator(vertexIndex) );
352 ApplyClampedForce( solverdt, force, vertexVelocity, inverseMass, vertexForce );
353 }
354 }
355 }
356 }
357 }
358 }
359 } // btCPUSoftBodySolver::applyForces
360
361 /**
362 * Integrate motion on the solver.
363 */
integrate(float solverdt)364 void btCPUSoftBodySolver::integrate( float solverdt )
365 {
366 using namespace Vectormath::Aos;
367 int numVertices = m_vertexData.getNumVertices();
368 for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex )
369 {
370 Point3 &position( m_vertexData.getPosition(vertexIndex) );
371 Point3 &previousPosition( m_vertexData.getPreviousPosition(vertexIndex) );
372 Vector3 &forceAccumulator( m_vertexData.getForceAccumulator(vertexIndex) );
373 Vector3 &velocity( m_vertexData.getVelocity(vertexIndex) );
374 float inverseMass = m_vertexData.getInverseMass(vertexIndex);
375
376 previousPosition = position;
377 velocity += forceAccumulator * inverseMass * solverdt;
378 position += velocity * solverdt;
379 forceAccumulator = Vector3(0.f, 0.f, 0.f);
380 }
381 } // btCPUSoftBodySolver::integrate
382
computeTriangleArea(const Vectormath::Aos::Point3 & vertex0,const Vectormath::Aos::Point3 & vertex1,const Vectormath::Aos::Point3 & vertex2)383 float btCPUSoftBodySolver::computeTriangleArea(
384 const Vectormath::Aos::Point3 &vertex0,
385 const Vectormath::Aos::Point3 &vertex1,
386 const Vectormath::Aos::Point3 &vertex2 )
387 {
388 Vectormath::Aos::Vector3 a = vertex1 - vertex0;
389 Vectormath::Aos::Vector3 b = vertex2 - vertex0;
390 Vectormath::Aos::Vector3 crossProduct = cross(a, b);
391 float area = length( crossProduct );
392 return area;
393 }
394
updateConstants(float timeStep)395 void btCPUSoftBodySolver::updateConstants( float timeStep )
396 {
397 using namespace Vectormath::Aos;
398
399 if( m_updateSolverConstants )
400 {
401 m_updateSolverConstants = false;
402
403 // Will have to redo this if we change the structure (tear, maybe) or various other possible changes
404
405 // Initialise link constants
406 const int numLinks = m_linkData.getNumLinks();
407 for( int linkIndex = 0; linkIndex < numLinks; ++linkIndex )
408 {
409 btSoftBodyLinkData::LinkNodePair &vertices( m_linkData.getVertexPair(linkIndex) );
410 m_linkData.getRestLength(linkIndex) = length((m_vertexData.getPosition( vertices.vertex0 ) - m_vertexData.getPosition( vertices.vertex1 )));
411 float invMass0 = m_vertexData.getInverseMass(vertices.vertex0);
412 float invMass1 = m_vertexData.getInverseMass(vertices.vertex1);
413 float linearStiffness = m_linkData.getLinearStiffnessCoefficient(linkIndex);
414 float massLSC = (invMass0 + invMass1)/linearStiffness;
415 m_linkData.getMassLSC(linkIndex) = massLSC;
416 float restLength = m_linkData.getRestLength(linkIndex);
417 float restLengthSquared = restLength*restLength;
418 m_linkData.getRestLengthSquared(linkIndex) = restLengthSquared;
419 }
420 }
421 } // btCPUSoftBodySolver::updateConstants
422
423
424
425
updateBounds()426 void btCPUSoftBodySolver::updateBounds()
427 {
428 using Vectormath::Aos::Point3;
429
430 for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
431 {
432 btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
433 btVector3 startBound(FLT_MAX, FLT_MAX, FLT_MAX);
434 btVector3 endBound(FLT_MIN, FLT_MIN, FLT_MIN);
435
436 const int startVertex = currentCloth->getFirstVertex();
437 const int numVertices = currentCloth->getNumVertices();
438
439 int endVertex = startVertex + numVertices;
440 for(int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex)
441 {
442 btVector3 vertexPosition( m_vertexData.getVertexPositions()[vertexIndex].getX(), m_vertexData.getVertexPositions()[vertexIndex].getY(), m_vertexData.getVertexPositions()[vertexIndex].getZ() );
443 startBound.setX( btMin( startBound.getX(), vertexPosition.getX() ) );
444 startBound.setY( btMin( startBound.getY(), vertexPosition.getY() ) );
445 startBound.setZ( btMin( startBound.getZ(), vertexPosition.getZ() ) );
446
447 endBound.setX( btMax( endBound.getX(), vertexPosition.getX() ) );
448 endBound.setY( btMax( endBound.getY(), vertexPosition.getY() ) );
449 endBound.setZ( btMax( endBound.getZ(), vertexPosition.getZ() ) );
450 }
451
452 m_softBodySet[clothIndex]->updateBounds( startBound, endBound );
453 }
454 }
455
456
457 class btCPUSB_QuickSortCompare
458 {
459 public:
460
operator ()(const btCPUCollisionShapeDescription & a,const btCPUCollisionShapeDescription & b)461 bool operator() ( const btCPUCollisionShapeDescription& a, const btCPUCollisionShapeDescription& b )
462 {
463 return ( a.softBodyIdentifier < b.softBodyIdentifier );
464 }
465 };
466
467 /**
468 * Sort the collision object details array and generate indexing into it for the per-cloth collision object array.
469 */
prepareCollisionConstraints()470 void btCPUSoftBodySolver::prepareCollisionConstraints()
471 {
472 // First do a simple sort on the collision objects
473 btAlignedObjectArray<int> numObjectsPerClothPrefixSum;
474 btAlignedObjectArray<int> numObjectsPerCloth;
475 numObjectsPerCloth.resize( m_softBodySet.size(), 0 );
476 numObjectsPerClothPrefixSum.resize( m_softBodySet.size(), 0 );
477
478 if (!m_perClothCollisionObjects.size())
479 return;
480
481 m_collisionObjectDetails.quickSort( btCPUSB_QuickSortCompare() );
482
483 // Generating indexing for perClothCollisionObjects
484 // First clear the previous values with the "no collision object for cloth" constant
485 for( int clothIndex = 0; clothIndex < m_perClothCollisionObjects.size(); ++clothIndex )
486 {
487 m_perClothCollisionObjects[clothIndex].firstObject = -1;
488 m_perClothCollisionObjects[clothIndex].endObject = -1;
489 }
490 int currentCloth = 0;
491 int startIndex = 0;
492 for( int collisionObject = 0; collisionObject < m_collisionObjectDetails.size(); ++collisionObject )
493 {
494 int nextCloth = m_collisionObjectDetails[collisionObject].softBodyIdentifier;
495 if( nextCloth != currentCloth )
496 {
497 // Changed cloth in the array
498 // Set the end index and the range is what we need for currentCloth
499 m_perClothCollisionObjects[currentCloth].firstObject = startIndex;
500 m_perClothCollisionObjects[currentCloth].endObject = collisionObject;
501 currentCloth = nextCloth;
502 startIndex = collisionObject;
503 }
504 }
505
506 // And update last cloth
507 m_perClothCollisionObjects[currentCloth].firstObject = startIndex;
508 m_perClothCollisionObjects[currentCloth].endObject = m_collisionObjectDetails.size();
509
510 } // prepareCollisionConstraints
511
512
solveConstraints(float solverdt)513 void btCPUSoftBodySolver::solveConstraints( float solverdt )
514 {
515 using Vectormath::Aos::Vector3;
516 using Vectormath::Aos::Point3;
517 using Vectormath::Aos::lengthSqr;
518 using Vectormath::Aos::dot;
519
520 // Prepare links
521 int numLinks = m_linkData.getNumLinks();
522 int numVertices = m_vertexData.getNumVertices();
523
524 float kst = 1.f;
525
526 for( int linkIndex = 0; linkIndex < numLinks; ++linkIndex )
527 {
528 btSoftBodyLinkData::LinkNodePair &nodePair( m_linkData.getVertexPair(linkIndex) );
529 Vector3 currentLength = m_vertexData.getPreviousPosition( nodePair.vertex1 ) - m_vertexData.getPreviousPosition( nodePair.vertex0 );
530 m_linkData.getCurrentLength(linkIndex) = currentLength;
531
532 // If mass at both ends of links is 0 (both static points) then we don't want this information.
533 // In reality this would be a fairly pointless link, but it could have been inserted
534 float linkLengthRatio = 0;
535 if( m_linkData.getMassLSC(linkIndex) > 0 )
536 linkLengthRatio = 1.f/(lengthSqr(currentLength) * m_linkData.getMassLSC(linkIndex));
537 m_linkData.getLinkLengthRatio(linkIndex) = linkLengthRatio;
538
539 }
540
541
542 prepareCollisionConstraints();
543
544
545 for( int iteration = 0; iteration < m_numberOfVelocityIterations ; ++iteration )
546 {
547 // Solve velocity
548 for(int linkIndex = 0; linkIndex < numLinks; ++linkIndex)
549 {
550
551 int vertexIndex0 = m_linkData.getVertexPair(linkIndex).vertex0;
552 int vertexIndex1 = m_linkData.getVertexPair(linkIndex).vertex1;
553
554 float j = -dot(m_linkData.getCurrentLength(linkIndex), m_vertexData.getVelocity(vertexIndex0) - m_vertexData.getVelocity(vertexIndex1)) * m_linkData.getLinkLengthRatio(linkIndex)*kst;
555
556 // If both ends of the link have no mass then this will be zero. Catch that case.
557 // TODO: Should really catch the /0 in the link setup, too
558 //if(psb->m_linksc0[i]>0)
559 {
560 m_vertexData.getVelocity(vertexIndex0) = m_vertexData.getVelocity(vertexIndex0) + m_linkData.getCurrentLength(linkIndex)*j*m_vertexData.getInverseMass(vertexIndex0);
561 m_vertexData.getVelocity(vertexIndex1) = m_vertexData.getVelocity(vertexIndex1) - m_linkData.getCurrentLength(linkIndex)*j*m_vertexData.getInverseMass(vertexIndex1);
562 }
563 }
564 }
565
566 // Compute new positions from velocity
567 // Also update the previous position so that our position computation is now based on the new position from the velocity solution
568 // rather than based directly on the original positions
569 if( m_numberOfVelocityIterations > 0 )
570 {
571 for(int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex)
572 {
573 m_vertexData.getPosition(vertexIndex) = m_vertexData.getPreviousPosition(vertexIndex) + m_vertexData.getVelocity(vertexIndex) * solverdt;
574 m_vertexData.getPreviousPosition(vertexIndex) = m_vertexData.getPosition(vertexIndex);
575 }
576 }
577
578 // Solve drift
579 for( int iteration = 0; iteration < m_numberOfPositionIterations ; ++iteration )
580 {
581 for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
582 {
583 btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
584
585 const int startLink = currentCloth->getFirstLink();
586 const int numLinks = currentCloth->getNumLinks();
587
588 int endLink = startLink + numLinks;
589 for(int linkIndex = startLink; linkIndex < endLink; ++linkIndex)
590 {
591 int vertexIndex0 = m_linkData.getVertexPair(linkIndex).vertex0;
592 int vertexIndex1 = m_linkData.getVertexPair(linkIndex).vertex1;
593
594 float massLSC = m_linkData.getMassLSC(linkIndex);
595 if( massLSC > 0.f )
596 {
597 Point3 &vertexPosition0( m_vertexData.getPosition( vertexIndex0 ) );
598 Point3 &vertexPosition1( m_vertexData.getPosition( vertexIndex1 ) );
599
600 Vector3 del = vertexPosition1 - vertexPosition0;
601 float len = lengthSqr(del);
602 float restLength2 = m_linkData.getRestLengthSquared(linkIndex);
603 float k = ((restLength2 - len) / (massLSC * (restLength2 + len) ) )*kst;
604
605 vertexPosition0 -= del*(k*m_vertexData.getInverseMass(vertexIndex0));
606 vertexPosition1 += del*(k*m_vertexData.getInverseMass(vertexIndex1));
607 }
608 }
609 }
610 }
611
612 // Clear forces so that friction is applied correctly
613 for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
614 {
615 btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
616
617 const int startLink = currentCloth->getFirstLink();
618 const int numLinks = currentCloth->getNumLinks();
619 const int startVertex = currentCloth->getFirstVertex();
620 const int numVertices = currentCloth->getNumVertices();
621 const int lastVertex = startVertex + numVertices;
622 // Update the velocities based on the change in position
623 // 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
624 float velocityCoefficient = (1.f - m_perClothDampingFactor[clothIndex]);
625 float velocityCorrectionCoefficient = m_perClothVelocityCorrectionCoefficient[clothIndex];
626 float isolverDt = 1.f/solverdt;
627
628 for(int vertexIndex = startVertex; vertexIndex < lastVertex; ++vertexIndex)
629 {
630 m_vertexData.getForceAccumulator( vertexIndex ) = Vector3(0.f, 0.f, 0.f);
631 }
632 }
633
634
635
636
637 // Solve collision constraints
638 // Very simple solver that pushes the vertex out of collision imposters for now
639 // to test integration with the broad phase code.
640 // May also want to put this into position solver loop every n iterations depending on
641 // how it behaves
642 for( int clothIndex = 0; clothIndex < m_softBodySet.size(); ++clothIndex )
643 {
644 btAcceleratedSoftBodyInterface *currentCloth = m_softBodySet[clothIndex];
645
646 float clothFriction = currentCloth->getSoftBody()->getFriction();
647
648 const int startVertex = currentCloth->getFirstVertex();
649 const int numVertices = currentCloth->getNumVertices();
650 int endVertex = startVertex + numVertices;
651
652 float velocityCoefficient = (1.f - m_perClothDampingFactor[clothIndex]);
653 float velocityCorrectionCoefficient = m_perClothVelocityCorrectionCoefficient[clothIndex];
654 float isolverDt = 1.f/solverdt;
655
656 int startObject = m_perClothCollisionObjects[clothIndex].firstObject;
657 int endObject = m_perClothCollisionObjects[clothIndex].endObject;
658
659 if( endObject == startObject )
660 {
661 // No collisions so just do the force update
662 for(int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex)
663 {
664 m_vertexData.getForceAccumulator( vertexIndex ) = Vector3(0.f, 0.f, 0.f);
665 }
666
667 // Recompute velocity based on updated position inclusive of drift
668 for(int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex)
669 {
670 m_vertexData.getVelocity(vertexIndex) = (m_vertexData.getPosition(vertexIndex) - m_vertexData.getPreviousPosition(vertexIndex)) * velocityCoefficient * isolverDt;
671 }
672 } else {
673
674 for( int collisionObject = startObject; collisionObject < endObject; ++collisionObject )
675 {
676 btCPUCollisionShapeDescription &shapeDescription( m_collisionObjectDetails[collisionObject] );
677
678 float colliderFriction = shapeDescription.friction;
679
680 if( shapeDescription.collisionShapeType == CAPSULE_SHAPE_PROXYTYPE )
681 {
682 using namespace Vectormath::Aos;
683
684 float capsuleHalfHeight = shapeDescription.shapeInformation.capsule.halfHeight;
685 float capsuleRadius = shapeDescription.shapeInformation.capsule.radius;
686 int capsuleUpAxis = shapeDescription.shapeInformation.capsule.upAxis;
687 float capsuleMargin = shapeDescription.margin;
688
689 Transform3 worldTransform = shapeDescription.shapeTransform;
690
691 // As this is a GPU comparison solver just iterate over the vertices
692 for( int vertexIndex = startVertex; vertexIndex < endVertex; ++vertexIndex )
693 {
694 // Clear force for vertex first
695 m_vertexData.getForceAccumulator( vertexIndex ) = Vector3(0.f, 0.f, 0.f);
696
697 Point3 vertex( m_vertexData.getPosition( vertexIndex ) );
698
699 // Correctly define the centerline depending on the upAxis
700 Point3 c1(0.f, 0.f, 0.f);
701 Point3 c2(0.f, 0.f, 0.f);
702 if( capsuleUpAxis == 0 ) {
703 c1.setX(-capsuleHalfHeight);
704 c2.setX(capsuleHalfHeight);
705 } else if( capsuleUpAxis == 1 ) {
706 c1.setY(-capsuleHalfHeight);
707 c2.setY(capsuleHalfHeight);
708 } else {
709 c1.setZ(-capsuleHalfHeight);
710 c2.setZ(capsuleHalfHeight);
711 }
712
713 Point3 worldC1 = worldTransform * c1;
714 Point3 worldC2 = worldTransform * c2;
715 Vector3 segment = worldC2 - worldC1;
716
717 // compute distance of tangent to vertex along line segment in capsule
718 float distanceAlongSegment = -( dot( worldC1 - vertex, segment ) / lengthSqr(segment) );
719
720 Point3 closestPoint = (worldC1 + segment * distanceAlongSegment);
721 float distanceFromLine = length(vertex - closestPoint);
722 float distanceFromC1 = length(worldC1 - vertex);
723 float distanceFromC2 = length(worldC2 - vertex);
724
725 // Final distance from collision, point to push from, direction to push in
726 // for impulse force
727 float distance;
728 Point3 sourcePoint;
729 Vector3 normalVector;
730 if( distanceAlongSegment < 0 )
731 {
732 distance = distanceFromC1;
733 sourcePoint = worldC1;
734 normalVector = normalize(vertex - worldC1);
735 } else if( distanceAlongSegment > 1.f ) {
736 distance = distanceFromC2;
737 sourcePoint = worldC2;
738 normalVector = normalize(vertex - worldC2);
739 } else {
740 distance = distanceFromLine;
741 sourcePoint = closestPoint;
742 normalVector = normalize(vertex - closestPoint);
743 }
744
745 Vector3 colliderLinearVelocity( shapeDescription.linearVelocity );
746 Vector3 colliderAngularVelocity( shapeDescription.angularVelocity );
747 Vector3 velocityOfSurfacePoint = colliderLinearVelocity + cross(colliderAngularVelocity, Vector3(vertex) - worldTransform.getTranslation());
748
749 float minDistance = capsuleRadius + capsuleMargin;
750 bool collided = false;
751
752 if( distance < minDistance )
753 {
754 // Project back to surface along normal
755 Vectormath::Aos::Point3 sourcePos = m_vertexData.getPosition( vertexIndex );
756 Vectormath::Aos::Vector3 posChange = (minDistance - distance)*normalVector*0.9;
757 //if( length(posChange) > 1 )
758 // std::cerr << "Poschange: " << length(posChange) << "\n";
759
760 Vectormath::Aos::Point3 newPos = sourcePos + posChange;
761 m_vertexData.getPosition( vertexIndex ) = newPos;
762 //m_vertexData.getPosition( vertexIndex ) = m_vertexData.getPosition( vertexIndex ) + (minDistance - distance)*normalVector*0.9;
763
764 // Experiment with moving back along source vector.
765 // Removes all ability to slide because it projects back along the vector length so it would undo lateral movement.
766 // TODO: This isn't quite right because we should take the movement of the collider into account as well
767 /*Vector3 incomingMoveVector( normalize(m_vertexData.getPreviousPosition(vertexIndex) - m_vertexData.getPosition(vertexIndex)) );
768 Vector3 normalDirectionMoveOut( (minDistance - distance)*normalVector*0.9 );
769 float distanceOnIncomingVector = dot(normalDirectionMoveOut, incomingMoveVector);
770 Vector3 vectorCorrection( distanceOnIncomingVector*incomingMoveVector );
771 m_vertexData.getPosition( vertexIndex ) = m_vertexData.getPosition( vertexIndex ) + vectorCorrection;*/
772
773
774 collided = true;
775 }
776
777 // Update velocity of vertex based on position
778 m_vertexData.getVelocity(vertexIndex) = (m_vertexData.getPosition(vertexIndex) - m_vertexData.getPreviousPosition(vertexIndex)) * velocityCoefficient * isolverDt;
779
780 // If we collided before we are on the surface so have friction
781 if( collided )
782 {
783 // Compute friction
784
785 // TODO: Just vertex velocity not enough, really we need the velocity
786 // relative to closest point on the surface of the collider
787 Vector3 vertexVelocity( m_vertexData.getVelocity(vertexIndex) );
788 Vector3 relativeVelocity( vertexVelocity - velocityOfSurfacePoint );
789
790
791 // First compute vectors for plane perpendicular to normal vector
792 // Cross any vector with normal vector first then cross the normal with it again
793 Vector3 p1(normalize(cross(normalVector, segment)));
794 Vector3 p2(normalize(cross(p1, normalVector)));
795 // Full friction is sum of velocities in each direction of plane.
796 Vector3 frictionVector(p1*dot(relativeVelocity, p1) + p2*dot(relativeVelocity, p2));
797
798 // Real friction is peak friction corrected by friction coefficients.
799 frictionVector = frictionVector*(colliderFriction*clothFriction);
800
801 float approachSpeed = dot( relativeVelocity, normalVector );
802
803 // For now just update vertex position by moving to radius distance along the push vector
804 // Could use this as the basis for simple vector distance constraint for the point later, possibly?
805 // That way in the main solver loop all shape types could be the same... though when
806 // we need to apply bi-directionally it becomes more complicated
807
808 // Add friction vector to the force accumulator
809 Vector3 ¤tForce( m_vertexData.getForceAccumulator( vertexIndex ) );
810
811 // Only apply if the vertex is moving towards the object to reduce jitter error
812 if( approachSpeed <= 0.0 )
813 currentForce -= frictionVector;
814 }
815 }
816 }
817 } // for( int collisionObject = startObject; collisionObject < endObject; ++collisionObject )
818 } // if( endObject == startObject )
819 }
820
821
822
823
824 } // btCPUSoftBodySolver::solveConstraints
825
826
findSoftBodyInterface(const btSoftBody * const softBody)827 btCPUSoftBodySolver::btAcceleratedSoftBodyInterface *btCPUSoftBodySolver::findSoftBodyInterface( const btSoftBody* const softBody )
828 {
829 for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
830 {
831 btAcceleratedSoftBodyInterface *softBodyInterface = m_softBodySet[softBodyIndex];
832 if( softBodyInterface->getSoftBody() == softBody )
833 return softBodyInterface;
834 }
835 return 0;
836 }
837
findSoftBodyInterface(const btSoftBody * const softBody) const838 const btCPUSoftBodySolver::btAcceleratedSoftBodyInterface * const btCPUSoftBodySolver::findSoftBodyInterface( const btSoftBody* const softBody ) const
839 {
840 for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
841 {
842 const btAcceleratedSoftBodyInterface *const softBodyInterface = m_softBodySet[softBodyIndex];
843 if( softBodyInterface->getSoftBody() == softBody )
844 return softBodyInterface;
845 }
846 return 0;
847 }
848
findSoftBodyIndex(const btSoftBody * const softBody)849 int btCPUSoftBodySolver::findSoftBodyIndex( const btSoftBody* const softBody )
850 {
851 for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
852 {
853 btAcceleratedSoftBodyInterface *softBodyInterface = m_softBodySet[softBodyIndex];
854 if( softBodyInterface->getSoftBody() == softBody )
855 return softBodyIndex;
856 }
857 return 1;
858 }
859
copySoftBodyToVertexBuffer(const btSoftBody * const softBody,btVertexBufferDescriptor * vertexBuffer)860 void btSoftBodySolverOutputCPUtoCPU::copySoftBodyToVertexBuffer( const btSoftBody * const softBody, btVertexBufferDescriptor *vertexBuffer )
861 {
862 // Currently only support CPU output buffers
863
864 const btSoftBodySolver *solver = softBody->getSoftBodySolver();
865 btAssert( solver->getSolverType() == btSoftBodySolver::CPU_SOLVER );
866 const btCPUSoftBodySolver *cpuSolver = static_cast< const btCPUSoftBodySolver * >( solver );
867 const btCPUSoftBodySolver::btAcceleratedSoftBodyInterface * const currentCloth = cpuSolver->findSoftBodyInterface( softBody );
868 const btSoftBodyVertexData &vertexData( cpuSolver->m_vertexData );
869
870 if( vertexBuffer->getBufferType() == btVertexBufferDescriptor::CPU_BUFFER )
871 {
872 const int firstVertex = currentCloth->getFirstVertex();
873 const int lastVertex = firstVertex + currentCloth->getNumVertices();
874 const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer);
875 float *basePointer = cpuVertexBuffer->getBasePointer();
876
877 if( vertexBuffer->hasVertexPositions() )
878 {
879 const int vertexOffset = cpuVertexBuffer->getVertexOffset();
880 const int vertexStride = cpuVertexBuffer->getVertexStride();
881 float *vertexPointer = basePointer + vertexOffset;
882
883 for( int vertexIndex = firstVertex; vertexIndex < lastVertex; ++vertexIndex )
884 {
885 Vectormath::Aos::Point3 position = vertexData.getPosition(vertexIndex);
886 *(vertexPointer + 0) = position.getX();
887 *(vertexPointer + 1) = position.getY();
888 *(vertexPointer + 2) = position.getZ();
889 vertexPointer += vertexStride;
890 }
891 }
892 if( vertexBuffer->hasNormals() )
893 {
894 const int normalOffset = cpuVertexBuffer->getNormalOffset();
895 const int normalStride = cpuVertexBuffer->getNormalStride();
896 float *normalPointer = basePointer + normalOffset;
897
898 for( int vertexIndex = firstVertex; vertexIndex < lastVertex; ++vertexIndex )
899 {
900 Vectormath::Aos::Vector3 normal = vertexData.getNormal(vertexIndex);
901 *(normalPointer + 0) = normal.getX();
902 *(normalPointer + 1) = normal.getY();
903 *(normalPointer + 2) = normal.getZ();
904 normalPointer += normalStride;
905 }
906 }
907 } else {
908 btAssert( 0=="Invalid vertex buffer descriptor used in CPU output." );
909 }
910 } // btCPUSoftBodySolver::outputToVertexBuffers
911
processCollision(btSoftBody *,btSoftBody *)912 void btCPUSoftBodySolver::processCollision( btSoftBody*, btSoftBody *)
913 {
914
915 }
916
917 // Add the collision object to the set to deal with for a particular soft body
processCollision(btSoftBody * softBody,btCollisionObject * collisionObject)918 void btCPUSoftBodySolver::processCollision( btSoftBody *softBody, btCollisionObject* collisionObject )
919 {
920 int softBodyIndex = findSoftBodyIndex( softBody );
921
922 if( softBodyIndex >= 0 )
923 {
924 btCollisionShape *collisionShape = collisionObject->getCollisionShape();
925 float friction = collisionObject->getFriction();
926 int shapeType = collisionShape->getShapeType();
927 if( shapeType == CAPSULE_SHAPE_PROXYTYPE )
928 {
929 // Add to the list of expected collision objects
930 btCPUCollisionShapeDescription newCollisionShapeDescription;
931 newCollisionShapeDescription.softBodyIdentifier = softBodyIndex;
932 newCollisionShapeDescription.collisionShapeType = shapeType;
933 newCollisionShapeDescription.shapeTransform = toTransform3(collisionObject->getWorldTransform());
934 btCapsuleShape *capsule = static_cast<btCapsuleShape*>( collisionShape );
935 newCollisionShapeDescription.shapeInformation.capsule.radius = capsule->getRadius();
936 newCollisionShapeDescription.shapeInformation.capsule.halfHeight = capsule->getHalfHeight();
937 newCollisionShapeDescription.shapeInformation.capsule.upAxis = capsule->getUpAxis();
938 newCollisionShapeDescription.margin = capsule->getMargin();
939 newCollisionShapeDescription.friction = friction;
940 btRigidBody* body = static_cast< btRigidBody* >( collisionObject );
941 newCollisionShapeDescription.linearVelocity = toVector3(body->getLinearVelocity());
942 newCollisionShapeDescription.angularVelocity = toVector3(body->getAngularVelocity());
943 m_collisionObjectDetails.push_back( newCollisionShapeDescription );
944 } else {
945 btAssert("Unsupported collision shape type\n");
946 }
947 } else {
948 btAssert("Unknown soft body");
949 }
950 } // btCPUSoftBodySolver::processCollision
951
952
predictMotion(float timeStep)953 void btCPUSoftBodySolver::predictMotion( float timeStep )
954 {
955 // Fill the force arrays with current acceleration data etc
956 m_perClothWindVelocity.resize( m_softBodySet.size() );
957 for( int softBodyIndex = 0; softBodyIndex < m_softBodySet.size(); ++softBodyIndex )
958 {
959 btSoftBody *softBody = m_softBodySet[softBodyIndex]->getSoftBody();
960
961 m_perClothWindVelocity[softBodyIndex] = toVector3(softBody->getWindVelocity());
962 }
963
964
965 // Apply forces that we know about to the cloths
966 applyForces( timeStep * getTimeScale() );
967
968 // Itegrate motion for all soft bodies dealt with by the solver
969 integrate( timeStep * getTimeScale() );
970
971 // Update bounds
972 // Will update the bounds for all softBodies being dealt with by the solver and
973 // set the values in the btSoftBody object
974 updateBounds();
975
976 // End prediction work for solvers
977 }
978
979
980