dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableLagrangianForce.h

   1 /*
   2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
   3
   4  Bullet Continuous Collision Detection and Physics Library
   5  Copyright (c) 2019 Google Inc. http://bulletphysics.org
   6  This software is provided 'as-is', without any express or implied warranty.
   7  In no event will the authors be held liable for any damages arising from the use of this software.
   8  Permission is granted to anyone to use this software for any purpose,
   9  including commercial applications, and to alter it and redistribute it freely,
  10  subject to the following restrictions:
  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 #ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H
  17 #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
  18
  19 #include "btSoftBody.h"
  20 #include <LinearMath/btHashMap.h>
  21 #include <iostream>
  22
  23 enum btDeformableLagrangianForceType
  24 {
  25         BT_GRAVITY_FORCE = 1,
  26         BT_MASSSPRING_FORCE = 2,
  27         BT_COROTATED_FORCE = 3,
  28         BT_NEOHOOKEAN_FORCE = 4,
  29         BT_LINEAR_ELASTICITY_FORCE = 5,
  30         BT_MOUSE_PICKING_FORCE = 6
  31 };
  32
  33 static inline double randomDouble(double low, double high)
  34 {
  35         return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
  36 }
  37
  38 class btDeformableLagrangianForce
  39 {
  40 public:
  41         typedef btAlignedObjectArray<btVector3> TVStack;
  42         btAlignedObjectArray<btSoftBody*> m_softBodies;
  43         const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
  44
  45         btDeformableLagrangianForce()
  46         {
  47         }
  48
  49         virtual ~btDeformableLagrangianForce() {}
  50
  51         // add all forces
  52         virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
  53
  54         // add damping df
  55         virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
  56
  57         // build diagonal of A matrix
  58         virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0;
  59
  60         // add elastic df
  61         virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
  62
  63         // add all forces that are explicit in explicit solve
  64         virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
  65
  66         // add all damping forces
  67         virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
  68
  69         virtual void addScaledHessian(btScalar scale) {}
  70
  71         virtual btDeformableLagrangianForceType getForceType() = 0;
  72
  73         virtual void reinitialize(bool nodeUpdated)
  74         {
  75         }
  76
  77         // get number of nodes that have the force
  78         virtual int getNumNodes()
  79         {
  80                 int numNodes = 0;
  81                 for (int i = 0; i < m_softBodies.size(); ++i)
  82                 {
  83                         numNodes += m_softBodies[i]->m_nodes.size();
  84                 }
  85                 return numNodes;
  86         }
  87
  88         // add a soft body to be affected by the particular lagrangian force
  89         virtual void addSoftBody(btSoftBody* psb)
  90         {
  91                 m_softBodies.push_back(psb);
  92         }
  93
  94         virtual void removeSoftBody(btSoftBody* psb)
  95         {
  96                 m_softBodies.remove(psb);
  97         }
  98
  99         virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
 100         {
 101                 m_nodes = nodes;
 102         }
 103
 104         // Calculate the incremental deformable generated from the input dx
 105         virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
 106         {
 107                 btVector3 c1 = dx[id1] - dx[id0];
 108                 btVector3 c2 = dx[id2] - dx[id0];
 109                 btVector3 c3 = dx[id3] - dx[id0];
 110                 return btMatrix3x3(c1, c2, c3).transpose();
 111         }
 112
 113         // Calculate the incremental deformable generated from the current velocity
 114         virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
 115         {
 116                 btVector3 c1 = n1->m_v - n0->m_v;
 117                 btVector3 c2 = n2->m_v - n0->m_v;
 118                 btVector3 c3 = n3->m_v - n0->m_v;
 119                 return btMatrix3x3(c1, c2, c3).transpose();
 120         }
 121
 122         // test for addScaledElasticForce function
 123         virtual void testDerivative()
 124         {
 125                 for (int i = 0; i < m_softBodies.size(); ++i)
 126                 {
 127                         btSoftBody* psb = m_softBodies[i];
 128                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 129                         {
 130                                 psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
 131                         }
 132                         psb->updateDeformation();
 133                 }
 134
 135                 TVStack dx;
 136                 dx.resize(getNumNodes());
 137                 TVStack dphi_dx;
 138                 dphi_dx.resize(dx.size());
 139                 for (int i = 0; i < dphi_dx.size(); ++i)
 140                 {
 141                         dphi_dx[i].setZero();
 142                 }
 143                 addScaledForces(-1, dphi_dx);
 144
 145                 // write down the current position
 146                 TVStack x;
 147                 x.resize(dx.size());
 148                 int counter = 0;
 149                 for (int i = 0; i < m_softBodies.size(); ++i)
 150                 {
 151                         btSoftBody* psb = m_softBodies[i];
 152                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 153                         {
 154                                 x[counter] = psb->m_nodes[j].m_q;
 155                                 counter++;
 156                         }
 157                 }
 158                 counter = 0;
 159
 160                 // populate dx with random vectors
 161                 for (int i = 0; i < dx.size(); ++i)
 162                 {
 163                         dx[i].setX(randomDouble(-1, 1));
 164                         dx[i].setY(randomDouble(-1, 1));
 165                         dx[i].setZ(randomDouble(-1, 1));
 166                 }
 167
 168                 btAlignedObjectArray<double> errors;
 169                 for (int it = 0; it < 10; ++it)
 170                 {
 171                         for (int i = 0; i < dx.size(); ++i)
 172                         {
 173                                 dx[i] *= 0.5;
 174                         }
 175
 176                         // get dphi/dx * dx
 177                         double dphi = 0;
 178                         for (int i = 0; i < dx.size(); ++i)
 179                         {
 180                                 dphi += dphi_dx[i].dot(dx[i]);
 181                         }
 182
 183                         for (int i = 0; i < m_softBodies.size(); ++i)
 184                         {
 185                                 btSoftBody* psb = m_softBodies[i];
 186                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 187                                 {
 188                                         psb->m_nodes[j].m_q = x[counter] + dx[counter];
 189                                         counter++;
 190                                 }
 191                                 psb->updateDeformation();
 192                         }
 193                         counter = 0;
 194                         double f1 = totalElasticEnergy(0);
 195
 196                         for (int i = 0; i < m_softBodies.size(); ++i)
 197                         {
 198                                 btSoftBody* psb = m_softBodies[i];
 199                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 200                                 {
 201                                         psb->m_nodes[j].m_q = x[counter] - dx[counter];
 202                                         counter++;
 203                                 }
 204                                 psb->updateDeformation();
 205                         }
 206                         counter = 0;
 207
 208                         double f2 = totalElasticEnergy(0);
 209
 210                         //restore m_q
 211                         for (int i = 0; i < m_softBodies.size(); ++i)
 212                         {
 213                                 btSoftBody* psb = m_softBodies[i];
 214                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 215                                 {
 216                                         psb->m_nodes[j].m_q = x[counter];
 217                                         counter++;
 218                                 }
 219                                 psb->updateDeformation();
 220                         }
 221                         counter = 0;
 222                         double error = f1 - f2 - 2 * dphi;
 223                         errors.push_back(error);
 224                         std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
 225                 }
 226                 for (int i = 1; i < errors.size(); ++i)
 227                 {
 228                         std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
 229                 }
 230         }
 231
 232         // test for addScaledElasticForce function
 233         virtual void testHessian()
 234         {
 235                 for (int i = 0; i < m_softBodies.size(); ++i)
 236                 {
 237                         btSoftBody* psb = m_softBodies[i];
 238                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 239                         {
 240                                 psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
 241                         }
 242                         psb->updateDeformation();
 243                 }
 244
 245                 TVStack dx;
 246                 dx.resize(getNumNodes());
 247                 TVStack df;
 248                 df.resize(dx.size());
 249                 TVStack f1;
 250                 f1.resize(dx.size());
 251                 TVStack f2;
 252                 f2.resize(dx.size());
 253
 254                 // write down the current position
 255                 TVStack x;
 256                 x.resize(dx.size());
 257                 int counter = 0;
 258                 for (int i = 0; i < m_softBodies.size(); ++i)
 259                 {
 260                         btSoftBody* psb = m_softBodies[i];
 261                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 262                         {
 263                                 x[counter] = psb->m_nodes[j].m_q;
 264                                 counter++;
 265                         }
 266                 }
 267                 counter = 0;
 268
 269                 // populate dx with random vectors
 270                 for (int i = 0; i < dx.size(); ++i)
 271                 {
 272                         dx[i].setX(randomDouble(-1, 1));
 273                         dx[i].setY(randomDouble(-1, 1));
 274                         dx[i].setZ(randomDouble(-1, 1));
 275                 }
 276
 277                 btAlignedObjectArray<double> errors;
 278                 for (int it = 0; it < 10; ++it)
 279                 {
 280                         for (int i = 0; i < dx.size(); ++i)
 281                         {
 282                                 dx[i] *= 0.5;
 283                         }
 284
 285                         // get df
 286                         for (int i = 0; i < df.size(); ++i)
 287                         {
 288                                 df[i].setZero();
 289                                 f1[i].setZero();
 290                                 f2[i].setZero();
 291                         }
 292
 293                         //set df
 294                         addScaledElasticForceDifferential(-1, dx, df);
 295
 296                         for (int i = 0; i < m_softBodies.size(); ++i)
 297                         {
 298                                 btSoftBody* psb = m_softBodies[i];
 299                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 300                                 {
 301                                         psb->m_nodes[j].m_q = x[counter] + dx[counter];
 302                                         counter++;
 303                                 }
 304                                 psb->updateDeformation();
 305                         }
 306                         counter = 0;
 307
 308                         //set f1
 309                         addScaledForces(-1, f1);
 310
 311                         for (int i = 0; i < m_softBodies.size(); ++i)
 312                         {
 313                                 btSoftBody* psb = m_softBodies[i];
 314                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 315                                 {
 316                                         psb->m_nodes[j].m_q = x[counter] - dx[counter];
 317                                         counter++;
 318                                 }
 319                                 psb->updateDeformation();
 320                         }
 321                         counter = 0;
 322
 323                         //set f2
 324                         addScaledForces(-1, f2);
 325
 326                         //restore m_q
 327                         for (int i = 0; i < m_softBodies.size(); ++i)
 328                         {
 329                                 btSoftBody* psb = m_softBodies[i];
 330                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 331                                 {
 332                                         psb->m_nodes[j].m_q = x[counter];
 333                                         counter++;
 334                                 }
 335                                 psb->updateDeformation();
 336                         }
 337                         counter = 0;
 338                         double error = 0;
 339                         for (int i = 0; i < df.size(); ++i)
 340                         {
 341                                 btVector3 error_vector = f1[i] - f2[i] - 2 * df[i];
 342                                 error += error_vector.length2();
 343                         }
 344                         error = btSqrt(error);
 345                         errors.push_back(error);
 346                         std::cout << "Iteration = " << it << ", error = " << error << std::endl;
 347                 }
 348                 for (int i = 1; i < errors.size(); ++i)
 349                 {
 350                         std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl;
 351                 }
 352         }
 353
 354         //
 355         virtual double totalElasticEnergy(btScalar dt)
 356         {
 357                 return 0;
 358         }
 359
 360         //
 361         virtual double totalDampingEnergy(btScalar dt)
 362         {
 363                 return 0;
 364         }
 365
 366         // total Energy takes dt as input because certain energies depend on dt
 367         virtual double totalEnergy(btScalar dt)
 368         {
 369                 return totalElasticEnergy(dt) + totalDampingEnergy(dt);
 370         }
 371 };
 372 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */