diff options
Diffstat (limited to 'source/gameengine/Ketsji/KX_ObjectActuator.cpp')
| -rw-r--r-- | source/gameengine/Ketsji/KX_ObjectActuator.cpp | 364 |
1 files changed, 304 insertions, 60 deletions
diff --git a/source/gameengine/Ketsji/KX_ObjectActuator.cpp b/source/gameengine/Ketsji/KX_ObjectActuator.cpp index 22a406792f9..98e73d4f0d7 100644 --- a/source/gameengine/Ketsji/KX_ObjectActuator.cpp +++ b/source/gameengine/Ketsji/KX_ObjectActuator.cpp @@ -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; @@ -187,18 +304,29 @@ PyParentObject KX_ObjectActuator::Parents[] = { }; PyMethodDef KX_ObjectActuator::Methods[] = { - {"getForce", (PyCFunction) KX_ObjectActuator::sPyGetForce, METH_VARARGS}, + {"getForce", (PyCFunction) KX_ObjectActuator::sPyGetForce, METH_NOARGS}, {"setForce", (PyCFunction) KX_ObjectActuator::sPySetForce, METH_VARARGS}, - {"getTorque", (PyCFunction) KX_ObjectActuator::sPyGetTorque, METH_VARARGS}, + {"getTorque", (PyCFunction) KX_ObjectActuator::sPyGetTorque, METH_NOARGS}, {"setTorque", (PyCFunction) KX_ObjectActuator::sPySetTorque, METH_VARARGS}, - {"getDLoc", (PyCFunction) KX_ObjectActuator::sPyGetDLoc, METH_VARARGS}, + {"getDLoc", (PyCFunction) KX_ObjectActuator::sPyGetDLoc, METH_NOARGS}, {"setDLoc", (PyCFunction) KX_ObjectActuator::sPySetDLoc, METH_VARARGS}, - {"getDRot", (PyCFunction) KX_ObjectActuator::sPyGetDRot, METH_VARARGS}, + {"getDRot", (PyCFunction) KX_ObjectActuator::sPyGetDRot, METH_NOARGS}, {"setDRot", (PyCFunction) KX_ObjectActuator::sPySetDRot, METH_VARARGS}, - {"getLinearVelocity", (PyCFunction) KX_ObjectActuator::sPyGetLinearVelocity, METH_VARARGS}, + {"getLinearVelocity", (PyCFunction) KX_ObjectActuator::sPyGetLinearVelocity, METH_NOARGS}, {"setLinearVelocity", (PyCFunction) KX_ObjectActuator::sPySetLinearVelocity, METH_VARARGS}, - {"getAngularVelocity", (PyCFunction) KX_ObjectActuator::sPyGetAngularVelocity, METH_VARARGS}, + {"getAngularVelocity", (PyCFunction) KX_ObjectActuator::sPyGetAngularVelocity, METH_NOARGS}, {"setAngularVelocity", (PyCFunction) KX_ObjectActuator::sPySetAngularVelocity, METH_VARARGS}, + {"setDamping", (PyCFunction) KX_ObjectActuator::sPySetDamping, METH_VARARGS}, + {"getDamping", (PyCFunction) KX_ObjectActuator::sPyGetDamping, METH_NOARGS}, + {"setForceLimitX", (PyCFunction) KX_ObjectActuator::sPySetForceLimitX, METH_VARARGS}, + {"getForceLimitX", (PyCFunction) KX_ObjectActuator::sPyGetForceLimitX, METH_NOARGS}, + {"setForceLimitY", (PyCFunction) KX_ObjectActuator::sPySetForceLimitY, METH_VARARGS}, + {"getForceLimitY", (PyCFunction) KX_ObjectActuator::sPyGetForceLimitY, METH_NOARGS}, + {"setForceLimitZ", (PyCFunction) KX_ObjectActuator::sPySetForceLimitZ, METH_VARARGS}, + {"getForceLimitZ", (PyCFunction) KX_ObjectActuator::sPyGetForceLimitZ, METH_NOARGS}, + {"setPID", (PyCFunction) KX_ObjectActuator::sPyGetPID, METH_NOARGS}, + {"getPID", (PyCFunction) KX_ObjectActuator::sPySetPID, METH_VARARGS}, + {NULL,NULL} //Sentinel @@ -212,9 +340,7 @@ PyObject* KX_ObjectActuator::_getattr(const STR_String& attr) { /* Removed! */ /* 2. getForce */ -PyObject* KX_ObjectActuator::PyGetForce(PyObject* self, - PyObject* args, - PyObject* kwds) +PyObject* KX_ObjectActuator::PyGetForce(PyObject* self) { PyObject *retVal = PyList_New(4); @@ -238,13 +364,12 @@ PyObject* KX_ObjectActuator::PySetForce(PyObject* self, } m_force.setValue(vecArg); m_bitLocalFlag.Force = PyArgToBool(bToggle); + UpdateFuzzyFlags(); Py_Return; } /* 4. getTorque */ -PyObject* KX_ObjectActuator::PyGetTorque(PyObject* self, - PyObject* args, - PyObject* kwds) +PyObject* KX_ObjectActuator::PyGetTorque(PyObject* self) { PyObject *retVal = PyList_New(4); @@ -268,13 +393,12 @@ PyObject* KX_ObjectActuator::PySetTorque(PyObject* self, } m_torque.setValue(vecArg); m_bitLocalFlag.Torque = PyArgToBool(bToggle); + UpdateFuzzyFlags(); Py_Return; } /* 6. getDLoc */ -PyObject* KX_ObjectActuator::PyGetDLoc(PyObject* self, - PyObject* args, - PyObject* kwds) +PyObject* KX_ObjectActuator::PyGetDLoc(PyObject* self) { PyObject *retVal = PyList_New(4); @@ -298,13 +422,12 @@ PyObject* KX_ObjectActuator::PySetDLoc(PyObject* self, } m_dloc.setValue(vecArg); m_bitLocalFlag.DLoc = PyArgToBool(bToggle); + UpdateFuzzyFlags(); Py_Return; } /* 8. getDRot */ -PyObject* KX_ObjectActuator::PyGetDRot(PyObject* self, - PyObject* args, - PyObject* kwds) +PyObject* KX_ObjectActuator::PyGetDRot(PyObject* self) { PyObject *retVal = PyList_New(4); @@ -328,13 +451,12 @@ PyObject* KX_ObjectActuator::PySetDRot(PyObject* self, } m_drot.setValue(vecArg); m_bitLocalFlag.DRot = PyArgToBool(bToggle); + UpdateFuzzyFlags(); Py_Return; } /* 10. getLinearVelocity */ -PyObject* KX_ObjectActuator::PyGetLinearVelocity(PyObject* self, - PyObject* args, - PyObject* kwds) { +PyObject* KX_ObjectActuator::PyGetLinearVelocity(PyObject* self) { PyObject *retVal = PyList_New(4); PyList_SetItem(retVal, 0, PyFloat_FromDouble(m_linear_velocity[0])); @@ -357,14 +479,13 @@ PyObject* KX_ObjectActuator::PySetLinearVelocity(PyObject* self, } m_linear_velocity.setValue(vecArg); m_bitLocalFlag.LinearVelocity = PyArgToBool(bToggle); + UpdateFuzzyFlags(); Py_Return; } /* 12. getAngularVelocity */ -PyObject* KX_ObjectActuator::PyGetAngularVelocity(PyObject* self, - PyObject* args, - PyObject* kwds) { +PyObject* KX_ObjectActuator::PyGetAngularVelocity(PyObject* self) { PyObject *retVal = PyList_New(4); PyList_SetItem(retVal, 0, PyFloat_FromDouble(m_angular_velocity[0])); @@ -386,10 +507,133 @@ 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) { + return Py_BuildValue("i",m_damping); +} +/* 6. getForceLimitX */ +PyObject* KX_ObjectActuator::PyGetForceLimitX(PyObject* self) +{ + 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 *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 *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 *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; } + /* eof */ |