Merged 15170:15635 from trunk (no conflicts or even merges)
[blender.git] / source / gameengine / Ketsji / KX_ObjectActuator.cpp
index 22a406792f9a708c2695a37f97d8ecf4595ff116..9ac0b4d47038855b92bfed9efc8424c8fc387b08 100644 (file)
@@ -50,6 +50,7 @@ KX_ObjectActuator(
        const MT_Vector3& drot,
        const MT_Vector3& linV,
        const MT_Vector3& angV,
+       const short damping,
        const KX_LocalFlags& flag,
        PyTypeObject* T
 ) : 
@@ -60,9 +61,23 @@ KX_ObjectActuator(
        m_drot(drot),
        m_linear_velocity(linV),
        m_angular_velocity(angV),
+       m_linear_length2(0.0),
+       m_current_linear_factor(0.0),
+       m_current_angular_factor(0.0),
+       m_damping(damping),
        m_bitLocalFlag (flag),
-       m_active_combined_velocity (false)
+       m_active_combined_velocity (false),
+       m_linear_damping_active(false),
+       m_angular_damping_active(false),
+       m_error_accumulator(0.0,0.0,0.0),
+       m_previous_error(0.0,0.0,0.0)
 {
+       if (m_bitLocalFlag.ServoControl)
+       {
+               // in servo motion, the force is local if the target velocity is local
+               m_bitLocalFlag.Force = m_bitLocalFlag.LinearVelocity;
+       }
+       UpdateFuzzyFlags();
 }
 
 bool KX_ObjectActuator::Update()
@@ -79,51 +94,153 @@ bool KX_ObjectActuator::Update()
                // it should reconcile the externally set velocity with it's 
                // own velocity.
                if (m_active_combined_velocity) {
-                       parent->ResolveCombinedVelocities(
-                                       m_linear_velocity,
-                                       m_angular_velocity,
-                                       (m_bitLocalFlag.LinearVelocity) != 0,
-                                       (m_bitLocalFlag.AngularVelocity) != 0
-                               );
+                       if (parent)
+                               parent->ResolveCombinedVelocities(
+                                               m_linear_velocity,
+                                               m_angular_velocity,
+                                               (m_bitLocalFlag.LinearVelocity) != 0,
+                                               (m_bitLocalFlag.AngularVelocity) != 0
+                                       );
                        m_active_combined_velocity = false;
                } 
+               m_linear_damping_active = false;
+               m_angular_damping_active = false;
+               m_error_accumulator.setValue(0.0,0.0,0.0);
+               m_previous_error.setValue(0.0,0.0,0.0);
                return false; 
 
-       } else 
-       if (parent)
+       } else if (parent)
        {
-               /* Probably better to use some flags, so these MT_zero tests can be  */
-               /* skipped.                                                          */
-               if (!MT_fuzzyZero(m_force))
-               {
-                       parent->ApplyForce(m_force,(m_bitLocalFlag.Force) != 0);
-               }
-               if (!MT_fuzzyZero(m_torque))
-               {
-                       parent->ApplyTorque(m_torque,(m_bitLocalFlag.Torque) != 0);
-               }
-               if (!MT_fuzzyZero(m_dloc))
+               if (m_bitLocalFlag.ServoControl) 
                {
-                       parent->ApplyMovement(m_dloc,(m_bitLocalFlag.DLoc) != 0);
-               }
-               if (!MT_fuzzyZero(m_drot))
-               {
-                       parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0);
-               }
-               if (!MT_fuzzyZero(m_linear_velocity))
+                       // In this mode, we try to reach a target speed using force
+                       // As we don't know the friction, we must implement a generic 
+                       // servo control to achieve the speed in a configurable
+                       // v = current velocity
+                       // V = target velocity
+                       // e = V-v = speed error
+                       // dt = time interval since previous update
+                       // I = sum(e(t)*dt)
+                       // dv = e(t) - e(t-1)
+                       // KP, KD, KI : coefficient
+                       // F = KP*e+KI*I+KD*dv
+                       MT_Scalar mass = parent->GetMass();
+                       if (mass < MT_EPSILON)
+                               return false;
+                       MT_Vector3 v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
+                       MT_Vector3 e = m_linear_velocity - v;
+                       MT_Vector3 dv = e - m_previous_error;
+                       MT_Vector3 I = m_error_accumulator + e;
+
+                       m_force = m_torque.x()*e+m_torque.y()*I+m_torque.z()*dv;
+                       // to automatically adapt the PID coefficient to mass;
+                       m_force *= mass;
+                       if (m_bitLocalFlag.Torque) 
+                       {
+                               if (m_force[0] > m_dloc[0])
+                               {
+                                       m_force[0] = m_dloc[0];
+                                       I[0] = m_error_accumulator[0];
+                               } else if (m_force[0] < m_drot[0])
+                               {
+                                       m_force[0] = m_drot[0];
+                                       I[0] = m_error_accumulator[0];
+                               }
+                       }
+                       if (m_bitLocalFlag.DLoc) 
+                       {
+                               if (m_force[1] > m_dloc[1])
+                               {
+                                       m_force[1] = m_dloc[1];
+                                       I[1] = m_error_accumulator[1];
+                               } else if (m_force[1] < m_drot[1])
+                               {
+                                       m_force[1] = m_drot[1];
+                                       I[1] = m_error_accumulator[1];
+                               }
+                       }
+                       if (m_bitLocalFlag.DRot) 
+                       {
+                               if (m_force[2] > m_dloc[2])
+                               {
+                                       m_force[2] = m_dloc[2];
+                                       I[2] = m_error_accumulator[2];
+                               } else if (m_force[2] < m_drot[2])
+                               {
+                                       m_force[2] = m_drot[2];
+                                       I[2] = m_error_accumulator[2];
+                               }
+                       }
+                       m_previous_error = e;
+                       m_error_accumulator = I;
+                       parent->ApplyForce(m_force,(m_bitLocalFlag.LinearVelocity) != 0);
+               } else
                {
-                       if (m_bitLocalFlag.AddOrSetLinV) {
-                               parent->addLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
-                       } else {
+                       if (!m_bitLocalFlag.ZeroForce)
+                       {
+                               parent->ApplyForce(m_force,(m_bitLocalFlag.Force) != 0);
+                       }
+                       if (!m_bitLocalFlag.ZeroTorque)
+                       {
+                               parent->ApplyTorque(m_torque,(m_bitLocalFlag.Torque) != 0);
+                       }
+                       if (!m_bitLocalFlag.ZeroDLoc)
+                       {
+                               parent->ApplyMovement(m_dloc,(m_bitLocalFlag.DLoc) != 0);
+                       }
+                       if (!m_bitLocalFlag.ZeroDRot)
+                       {
+                               parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0);
+                       }
+                       if (!m_bitLocalFlag.ZeroLinearVelocity)
+                       {
+                               if (m_bitLocalFlag.AddOrSetLinV) {
+                                       parent->addLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
+                               } else {
+                                       m_active_combined_velocity = true;
+                                       if (m_damping > 0) {
+                                               MT_Vector3 linV;
+                                               if (!m_linear_damping_active) {
+                                                       // delta and the start speed (depends on the existing speed in that direction)
+                                                       linV = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
+                                                       // keep only the projection along the desired direction
+                                                       m_current_linear_factor = linV.dot(m_linear_velocity)/m_linear_length2;
+                                                       m_linear_damping_active = true;
+                                               }
+                                               if (m_current_linear_factor < 1.0)
+                                                       m_current_linear_factor += 1.0/m_damping;
+                                               if (m_current_linear_factor > 1.0)
+                                                       m_current_linear_factor = 1.0;
+                                               linV = m_current_linear_factor * m_linear_velocity;
+                                               parent->setLinearVelocity(linV,(m_bitLocalFlag.LinearVelocity) != 0);
+                                       } else {
+                                               parent->setLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
+                                       }
+                               }
+                       }
+                       if (!m_bitLocalFlag.ZeroAngularVelocity)
+                       {
                                m_active_combined_velocity = true;
-                               parent->setLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
+                               if (m_damping > 0) {
+                                       MT_Vector3 angV;
+                                       if (!m_angular_damping_active) {
+                                               // delta and the start speed (depends on the existing speed in that direction)
+                                               angV = parent->GetAngularVelocity(m_bitLocalFlag.AngularVelocity);
+                                               // keep only the projection along the desired direction
+                                               m_current_angular_factor = angV.dot(m_angular_velocity)/m_angular_length2;
+                                               m_angular_damping_active = true;
+                                       }
+                                       if (m_current_angular_factor < 1.0)
+                                               m_current_angular_factor += 1.0/m_damping;
+                                       if (m_current_angular_factor > 1.0)
+                                               m_current_angular_factor = 1.0;
+                                       angV = m_current_angular_factor * m_angular_velocity;
+                                       parent->setAngularVelocity(angV,(m_bitLocalFlag.AngularVelocity) != 0);
+                               } else {
+                                       parent->setAngularVelocity(m_angular_velocity,(m_bitLocalFlag.AngularVelocity) != 0);
+                               }
                        }
                }
-               if (!MT_fuzzyZero(m_angular_velocity))
-               {
-                       parent->setAngularVelocity(m_angular_velocity,(m_bitLocalFlag.AngularVelocity) != 0);
-                       m_active_combined_velocity = true;
-               }
                
        }
        return true;
@@ -199,6 +316,17 @@ PyMethodDef KX_ObjectActuator::Methods[] = {
        {"setLinearVelocity", (PyCFunction) KX_ObjectActuator::sPySetLinearVelocity, METH_VARARGS},
        {"getAngularVelocity", (PyCFunction) KX_ObjectActuator::sPyGetAngularVelocity, METH_VARARGS},
        {"setAngularVelocity", (PyCFunction) KX_ObjectActuator::sPySetAngularVelocity, METH_VARARGS},
+       {"setDamping", (PyCFunction) KX_ObjectActuator::sPySetDamping, METH_VARARGS},
+       {"getDamping", (PyCFunction) KX_ObjectActuator::sPyGetDamping, METH_VARARGS},
+       {"setForceLimitX", (PyCFunction) KX_ObjectActuator::sPySetForceLimitX, METH_VARARGS},
+       {"getForceLimitX", (PyCFunction) KX_ObjectActuator::sPyGetForceLimitX, METH_VARARGS},
+       {"setForceLimitY", (PyCFunction) KX_ObjectActuator::sPySetForceLimitY, METH_VARARGS},
+       {"getForceLimitY", (PyCFunction) KX_ObjectActuator::sPyGetForceLimitY, METH_VARARGS},
+       {"setForceLimitZ", (PyCFunction) KX_ObjectActuator::sPySetForceLimitZ, METH_VARARGS},
+       {"getForceLimitZ", (PyCFunction) KX_ObjectActuator::sPyGetForceLimitZ, METH_VARARGS},
+       {"setPID", (PyCFunction) KX_ObjectActuator::sPyGetPID, METH_VARARGS},
+       {"getPID", (PyCFunction) KX_ObjectActuator::sPySetPID, METH_VARARGS},
+
 
 
        {NULL,NULL} //Sentinel
@@ -238,6 +366,7 @@ PyObject* KX_ObjectActuator::PySetForce(PyObject* self,
        }
        m_force.setValue(vecArg);
        m_bitLocalFlag.Force = PyArgToBool(bToggle);
+       UpdateFuzzyFlags();
        Py_Return;
 }
 
@@ -268,6 +397,7 @@ PyObject* KX_ObjectActuator::PySetTorque(PyObject* self,
        }
        m_torque.setValue(vecArg);
        m_bitLocalFlag.Torque = PyArgToBool(bToggle);
+       UpdateFuzzyFlags();
        Py_Return;
 }
 
@@ -298,6 +428,7 @@ PyObject* KX_ObjectActuator::PySetDLoc(PyObject* self,
        }
        m_dloc.setValue(vecArg);
        m_bitLocalFlag.DLoc = PyArgToBool(bToggle);
+       UpdateFuzzyFlags();
        Py_Return;
 }
 
@@ -328,6 +459,7 @@ PyObject* KX_ObjectActuator::PySetDRot(PyObject* self,
        }
        m_drot.setValue(vecArg);
        m_bitLocalFlag.DRot = PyArgToBool(bToggle);
+       UpdateFuzzyFlags();
        Py_Return;
 }
 
@@ -357,6 +489,7 @@ PyObject* KX_ObjectActuator::PySetLinearVelocity(PyObject* self,
        }
        m_linear_velocity.setValue(vecArg);
        m_bitLocalFlag.LinearVelocity = PyArgToBool(bToggle);
+       UpdateFuzzyFlags();
        Py_Return;
 }
 
@@ -386,9 +519,142 @@ PyObject* KX_ObjectActuator::PySetAngularVelocity(PyObject* self,
        }
        m_angular_velocity.setValue(vecArg);
        m_bitLocalFlag.AngularVelocity = PyArgToBool(bToggle);
+       UpdateFuzzyFlags();
+       Py_Return;
+}
+
+/* 13. setDamping                                                */
+PyObject* KX_ObjectActuator::PySetDamping(PyObject* self, 
+                                                                                 PyObject* args, 
+                                                                                 PyObject* kwds) {
+       int damping = 0;
+       if (!PyArg_ParseTuple(args, "i", &damping) || damping < 0 || damping > 1000) {
+               return NULL;
+       }
+       m_damping = damping;
+       Py_Return;
+}
+
+/* 13. getVelocityDamping                                                */
+PyObject* KX_ObjectActuator::PyGetDamping(PyObject* self, 
+                                                                                 PyObject* args, 
+                                                                                 PyObject* kwds) {
+       return Py_BuildValue("i",m_damping);
+}
+/* 6. getForceLimitX                                                                */
+PyObject* KX_ObjectActuator::PyGetForceLimitX(PyObject* self, 
+                                                                                         PyObject* args, 
+                                                                                         PyObject* kwds)
+{
+       PyObject *retVal = PyList_New(3);
+
+       PyList_SetItem(retVal, 0, PyFloat_FromDouble(m_drot[0]));
+       PyList_SetItem(retVal, 1, PyFloat_FromDouble(m_dloc[0]));
+       PyList_SetItem(retVal, 2, BoolToPyArg(m_bitLocalFlag.Torque));
+       
+       return retVal;
+}
+/* 7. setForceLimitX                                                         */
+PyObject* KX_ObjectActuator::PySetForceLimitX(PyObject* self, 
+                                                                                         PyObject* args, 
+                                                                                         PyObject* kwds)
+{
+       float vecArg[2];
+       int bToggle = 0;
+       if(!PyArg_ParseTuple(args, "ffi", &vecArg[0], &vecArg[1], &bToggle)) {
+               return NULL;
+       }
+       m_drot[0] = vecArg[0];
+       m_dloc[0] = vecArg[1];
+       m_bitLocalFlag.Torque = PyArgToBool(bToggle);
+       Py_Return;
+}
+
+/* 6. getForceLimitY                                                                */
+PyObject* KX_ObjectActuator::PyGetForceLimitY(PyObject* self, 
+                                                                                         PyObject* args, 
+                                                                                         PyObject* kwds)
+{
+       PyObject *retVal = PyList_New(3);
+
+       PyList_SetItem(retVal, 0, PyFloat_FromDouble(m_drot[1]));
+       PyList_SetItem(retVal, 1, PyFloat_FromDouble(m_dloc[1]));
+       PyList_SetItem(retVal, 2, BoolToPyArg(m_bitLocalFlag.DLoc));
+       
+       return retVal;
+}
+/* 7. setForceLimitY                                                                */
+PyObject* KX_ObjectActuator::PySetForceLimitY(PyObject* self, 
+                                                                                         PyObject* args, 
+                                                                                         PyObject* kwds)
+{
+       float vecArg[2];
+       int bToggle = 0;
+       if(!PyArg_ParseTuple(args, "ffi", &vecArg[0], &vecArg[1], &bToggle)) {
+               return NULL;
+       }
+       m_drot[1] = vecArg[0];
+       m_dloc[1] = vecArg[1];
+       m_bitLocalFlag.DLoc = PyArgToBool(bToggle);
+       Py_Return;
+}
+
+/* 6. getForceLimitZ                                                                */
+PyObject* KX_ObjectActuator::PyGetForceLimitZ(PyObject* self, 
+                                                                                         PyObject* args, 
+                                                                                         PyObject* kwds)
+{
+       PyObject *retVal = PyList_New(3);
+
+       PyList_SetItem(retVal, 0, PyFloat_FromDouble(m_drot[2]));
+       PyList_SetItem(retVal, 1, PyFloat_FromDouble(m_dloc[2]));
+       PyList_SetItem(retVal, 2, BoolToPyArg(m_bitLocalFlag.DRot));
+       
+       return retVal;
+}
+/* 7. setForceLimitZ                                                                */
+PyObject* KX_ObjectActuator::PySetForceLimitZ(PyObject* self, 
+                                                                                         PyObject* args, 
+                                                                                         PyObject* kwds)
+{
+       float vecArg[2];
+       int bToggle = 0;
+       if(!PyArg_ParseTuple(args, "ffi", &vecArg[0], &vecArg[1], &bToggle)) {
+               return NULL;
+       }
+       m_drot[2] = vecArg[0];
+       m_dloc[2] = vecArg[1];
+       m_bitLocalFlag.DRot = PyArgToBool(bToggle);
        Py_Return;
 }
 
+/* 4. getPID                                                              */
+PyObject* KX_ObjectActuator::PyGetPID(PyObject* self, 
+                                                                         PyObject* args, 
+                                                                         PyObject* kwds)
+{
+       PyObject *retVal = PyList_New(3);
+
+       PyList_SetItem(retVal, 0, PyFloat_FromDouble(m_torque[0]));
+       PyList_SetItem(retVal, 1, PyFloat_FromDouble(m_torque[1]));
+       PyList_SetItem(retVal, 2, PyFloat_FromDouble(m_torque[2]));
+       
+       return retVal;
+}
+/* 5. setPID                                                              */
+PyObject* KX_ObjectActuator::PySetPID(PyObject* self, 
+                                                                         PyObject* args, 
+                                                                         PyObject* kwds)
+{
+       float vecArg[3];
+       if (!PyArg_ParseTuple(args, "fff", &vecArg[0], &vecArg[1], &vecArg[2])) {
+               return NULL;
+       }
+       m_torque.setValue(vecArg);
+       Py_Return;
+}
+
+