clears issue related to order of the sceneobjects in Coll. detection!

include/smCollision/smCollisionPair.h

@@ -41,9 +41,10 @@ class smContact
 {
 public:
     smContact ( const double penetrationDepth,
-                const smVec3d& contactPoint,
+                const smVec3d& p,
+                const int ind,
                 const smVec3d& contactNornmal
-				) : depth(penetrationDepth), point(contactPoint), normal(contactNornmal){}
+				) : depth(penetrationDepth), point(p), index(ind), normal(contactNornmal){}
 
     double depth;
     smVec3d point;
@@ -77,8 +78,9 @@ public:
     /// @brief Add contact between the models
     ///
     void addContact( const double& penetrationDepth,
-                      const smVec3d& contactPoint,
-                      const smVec3d& contactNornmal);
+                     const smVec3d& vert,
+                     const int index,
+                     const smVec3d& contactNornmal);
 
     ///
     /// @brief Clear contact list

src/smCollision/UnitTests/CollisionPairSpec.cpp

@@ -53,7 +53,7 @@ go_bandit([](){
             float depth = 1.0;
             smVec3d contactPoint(0,0,1);
             smVec3d normal(1,0,0);
-            collisionPair->addContact(depth,contactPoint,normal);
+            collisionPair->addContact(depth,contactPoint,1,normal);
 
             std::shared_ptr<smContact> contact = collisionPair->getContacts().back();
 

src/smCollision/smCollisionPair.cpp

@@ -35,9 +35,10 @@ void smCollisionPair::setModels(const std::shared_ptr< smModelRepresentation >&
     this->modelRepresentations.second = second;
 }
 
-void smCollisionPair::addContact(const double& penetrationDepth, const smVec3d& contactPoint, const smVec3d& contactNornmal)
+void smCollisionPair::addContact(const double& penetrationDepth, const smVec3d& vert, const int index, const smVec3d& contactNornmal)
 {
-    auto contact = std::make_shared<smContact>(penetrationDepth, contactPoint, contactNornmal);
+    auto contact = std::make_shared<smContact>(penetrationDepth, vert, index, contactNornmal);
+    //std::shared_ptr<smContact> contact(new smContact(penetrationDepth, vert, index, contactNornmal));
     this->contacts.emplace_back(contact);
 }
 

src/smCollision/smMeshToMeshCollision.cpp

@@ -77,7 +77,7 @@ void smMeshToMeshCollision::doComputeCollision(std::shared_ptr<smCollisionPair>
                                               depth, contactPoint, normal)
                   )
                 {
-                    pairs->addContact(std::abs(depth), contactPoint, normal);
+                    //pairs->addContact(std::abs(depth), contactPoint, normal);
                 }
             }
         }

src/smCollision/smPlaneToMeshCollision.cpp

@@ -55,10 +55,9 @@ void smPlaneToMeshCollision::doComputeCollision(std::shared_ptr<smCollisionPair>
 
         if (d < std::numeric_limits<float>::epsilon())
         {         
-            pair->addContact(d, vert, planeNormal);// Create contact
+            pair->addContact(d, vert, i, planeNormal);// Create contact
+
+            std::cout << "Pushed contact\n";
         }
     }
-
-    std::cout << "# contacts: " << pair->getNumberOfContacts() << std::endl;
-
 }
\ No newline at end of file

src/smContactHandling/UnitTests/PenaltyContactHandlingSpec.cpp

@@ -55,7 +55,7 @@ std::shared_ptr<smCollisionPair> createSampleCollisionPair()
     smVec3d contactPoint(0,0,1);
     smVec3d normal(1,0,0);
 
-    collisionPair->addContact(depth,contactPoint,normal);
+    collisionPair->addContact(depth,contactPoint,1,normal);
 
 	collisionPair->getContacts()[0]->index = 0;
 

src/smContactHandling/smPenaltyContactHandling.cpp

@@ -72,56 +72,40 @@ void smPenaltyContactHandling::resolveContacts()
 
 void smPenaltyContactHandling::computeUnilateralContactForces()
 {
-    auto femSceneObject = std::static_pointer_cast<smVegaFemSceneObject>(this->getFirstSceneObject());
-
-    std::cout << femSceneObject->f_contact.size() << std::endl;
-
-    femSceneObject->f_contact[2] = 1.0e5;  return;
-
-
-
-
-
-
-
-
-
     int penetratedNode, nodeDofID;
     const double stiffness = 1.0e5, damping = 2000.0;
     smVec3d velocityProjection;
 
     auto contactInfo = this->getCollisionPairs()->getContacts();
 
-    if( this->getFirstSceneObject()->getType() == SIMMEDTK_SMVEGAFEMSCENEOBJECT
-            && this->getSecondSceneObject()->getType() == SIMMEDTK_SMSTATICSCENEOBJECT )
-    {
-        auto femSceneObject = std::static_pointer_cast<smVegaFemSceneObject>( this->getFirstSceneObject() );
+    std::cout << "# contacts: " << contactInfo.size() << std::endl;
 
-        femSceneObject->f_contact[2] = 1.0e5; return;
+    if (this->getSecondSceneObject()->getType() == SIMMEDTK_SMVEGAFEMSCENEOBJECT &&
+        this->getFirstSceneObject()->getType() == SIMMEDTK_SMSTATICSCENEOBJECT)
+    {
+        auto femSceneObject = std::static_pointer_cast<smVegaFemSceneObject>( this->getSecondSceneObject() );
 
-        femSceneObject->setContactForcesToZero();
+        femSceneObject->setContactForcesToZero(); 
         smVec3d force;
         for( int i = 0; i < contactInfo.size(); i++ )
         {
+
             nodeDofID = 3 * contactInfo[i]->index;
 
             velocityProjection = smVec3d( femSceneObject->uvel[nodeDofID], femSceneObject->uvel[nodeDofID + 1], femSceneObject->uvel[nodeDofID + 2] );
             velocityProjection = contactInfo[i]->normal.dot( velocityProjection ) * contactInfo[i]->normal;
 
-            force = stiffness * contactInfo[i]->depth * contactInfo[i]->normal - damping * velocityProjection;
+            force = stiffness * -contactInfo[i]->depth * contactInfo[i]->normal - damping * velocityProjection;
 
-            femSceneObject->f_contact[nodeDofID] += 0.0;// force(0);
-            femSceneObject->f_contact[nodeDofID + 1] += 0.0;// force(1);
-            femSceneObject->f_contact[nodeDofID + 2] += 0.0;// force( 2 );
+            femSceneObject->f_contact[nodeDofID] += force(0);
+            femSceneObject->f_contact[nodeDofID + 1] += force(1);
+            femSceneObject->f_contact[nodeDofID + 2] += force(2);
 
         }
-        femSceneObject->applyContactForces();
     }
 
 }
 
 void smPenaltyContactHandling::computeBilateralContactForces()
 {
-
-
 }

src/smSimulators/smVegaFemSceneObject.cpp

@@ -1005,7 +1005,7 @@ void smVegaFemSceneObject::advanceDynamics()
         applyUserInteractionForces();
 
         // apply pre-defined external forces
-        applyScriptedExternalForces();
+        //applyScriptedExternalForces();
 
         // apply external forces arising from contact
         applyContactForces();