From d435db293105f9319fc8865558bd0edac104d318 Mon Sep 17 00:00:00 2001
From: Leandro Dorileo <dorileo@profusion.mobi>
Date: Wed, 5 Dec 2012 13:50:08 +0000
Subject: [PATCH] EPhysics: fix soft body velocity and stop implementation

+ velocity
Handle cloth cases on velocity functions and consider both rigid
and soft body whenever touching velocities + stoping a body.

+ activation
Activate the body when setting angular velocity.

--This line, and those below, will be ignored--


SVN revision: 80248
---
 legacy/ephysics/src/lib/EPhysics.h        |  2 +
 legacy/ephysics/src/lib/ephysics_body.cpp | 68 +++++++++++++++++++++++++++----
 2 files changed, 63 insertions(+), 7 deletions(-)

diff --git a/legacy/ephysics/src/lib/EPhysics.h b/legacy/ephysics/src/lib/EPhysics.h
index 1675fe3..5a3f9b1 100644
--- a/legacy/ephysics/src/lib/EPhysics.h
+++ b/legacy/ephysics/src/lib/EPhysics.h
@@ -3282,6 +3282,8 @@ EAPI void ephysics_body_linear_velocity_set(EPhysics_Body *body, double x, doubl
  * @param z The linear velocity on axis z.
  *
  * @note EPhysics unit for linear velocity is Evas coordinates per second.
+ * @note For cloth bodies the returned value is a velocity average of nodes
+ * velocities.
  *
  * @see ephysics_body_linear_velocity_set().
  * @see ephysics_body_angular_velocity_get().
diff --git a/legacy/ephysics/src/lib/ephysics_body.cpp b/legacy/ephysics/src/lib/ephysics_body.cpp
index 27f6079..f6b63e8 100644
--- a/legacy/ephysics/src/lib/ephysics_body.cpp
+++ b/legacy/ephysics/src/lib/ephysics_body.cpp
@@ -534,9 +534,17 @@ _ephysics_body_sleeping_threshold_set(EPhysics_Body *body, double linear_thresho
 static inline void
 _ephysics_body_linear_velocity_set(EPhysics_Body *body, double x, double y, double z, double rate)
 {
+   btVector3 linear_velocity = btVector3(x / rate, -y / rate, z / rate);
+
    ephysics_body_activate(body, EINA_TRUE);
-   body->rigid_body->setLinearVelocity(btVector3(x / rate, -y / rate,
-                                                 z / rate));
+   if (body->rigid_body)
+     body->rigid_body->setLinearVelocity(linear_velocity);
+
+   if (body->soft_body)
+     {
+        for (int i = 0; i < body->soft_body->m_nodes.size(); i++)
+          body->soft_body->m_nodes[i].m_v = linear_velocity;
+     }
 }
 
 static void
@@ -3425,6 +3433,21 @@ ephysics_body_linear_velocity_set(EPhysics_Body *body, double x, double y, doubl
    DBG("Linear velocity of body %p set to (%lf, %lf, %lf).", body, x, y, z);
 }
 
+static void
+_ephysics_body_soft_body_linear_velocity_get(const EPhysics_Body *body, double *x, double *y, double *z, double rate)
+{
+   btVector3 total_velocity = btVector3(0, 0, 0);
+   int nodes_size = body->soft_body->m_nodes.size();
+
+   for (int i = 0; i < nodes_size; i++)
+     total_velocity += body->soft_body->m_nodes[i].m_v;
+
+   total_velocity /= nodes_size;
+   if (x) *x = total_velocity.getX() * rate;
+   if (y) *y = -total_velocity.getY() * rate;
+   if (z) *z = total_velocity.getZ() * rate;
+}
+
 EAPI void
 ephysics_body_linear_velocity_get(const EPhysics_Body *body, double *x, double *y, double *z)
 {
@@ -3437,9 +3460,14 @@ ephysics_body_linear_velocity_get(const EPhysics_Body *body, double *x, double *
      }
 
    rate = ephysics_world_rate_get(body->world);
-   if (x) *x = body->rigid_body->getLinearVelocity().getX() * rate;
-   if (y) *y = -body->rigid_body->getLinearVelocity().getY() * rate;
-   if (z) *z = body->rigid_body->getLinearVelocity().getZ() * rate;
+   if (body->rigid_body)
+     {
+        if (x) *x = body->rigid_body->getLinearVelocity().getX() * rate;
+        if (y) *y = -body->rigid_body->getLinearVelocity().getY() * rate;
+        if (z) *z = body->rigid_body->getLinearVelocity().getZ() * rate;
+        return;
+     }
+   _ephysics_body_soft_body_linear_velocity_get(body, x, y, z, rate);
 }
 
 EAPI void
@@ -3451,10 +3479,18 @@ ephysics_body_angular_velocity_set(EPhysics_Body *body, double x, double y, doub
         return;
      }
 
+   if (body->type == EPHYSICS_BODY_TYPE_CLOTH)
+     {
+        ERR("Can't set angular velocity, not implemented for cloth.");
+        return;
+     }
+
    ephysics_world_lock_take(body->world);
+   ephysics_body_activate(body, EINA_TRUE);
    body->rigid_body->setAngularVelocity(btVector3(-x / RAD_TO_DEG,
                                                   -y / RAD_TO_DEG,
                                                   -z/RAD_TO_DEG));
+
    DBG("Angular velocity of body %p set to (%lf, %lf, %lf).", body, x, y, z);
    ephysics_world_lock_release(body->world);
 }
@@ -3468,6 +3504,12 @@ ephysics_body_angular_velocity_get(const EPhysics_Body *body, double *x, double
         return;
      }
 
+   if (body->type == EPHYSICS_BODY_TYPE_CLOTH)
+     {
+        ERR("Can't get angular velocity, not implemented for cloth.");
+        return;
+     }
+
    if (x) *x = -body->rigid_body->getAngularVelocity().getX() * RAD_TO_DEG;
    if (y) *y = -body->rigid_body->getAngularVelocity().getY() * RAD_TO_DEG;
    if (z) *z = -body->rigid_body->getAngularVelocity().getZ() * RAD_TO_DEG;
@@ -3518,8 +3560,20 @@ ephysics_body_stop(EPhysics_Body *body)
      }
 
    ephysics_world_lock_take(body->world);
-   body->rigid_body->setLinearVelocity(btVector3(0, 0, 0));
-   body->rigid_body->setAngularVelocity(btVector3(0, 0, 0));
+   if (body->rigid_body)
+     {
+        body->rigid_body->setLinearVelocity(btVector3(0, 0, 0));
+        body->rigid_body->setAngularVelocity(btVector3(0, 0, 0));
+     }
+
+   if (body->soft_body)
+     {
+        for (int i = 0; i < body->soft_body->m_nodes.size(); i++)
+          {
+             body->soft_body->m_nodes[i].m_v *= 0;
+             body->soft_body->m_nodes[i].m_f *= 0;
+          }
+     }
    ephysics_world_lock_release(body->world);
 
    DBG("Body %p stopped", body);
-- 
2.7.4