summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/joints/amotor.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/src/joints/amotor.cpp')
-rw-r--r--libs/ode-0.16.1/ode/src/joints/amotor.cpp810
1 files changed, 810 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/joints/amotor.cpp b/libs/ode-0.16.1/ode/src/joints/amotor.cpp
new file mode 100644
index 0000000..aa30c76
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/amotor.cpp
@@ -0,0 +1,810 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ@q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "common.h"
+#include "amotor.h"
+#include "joint_internal.h"
+#include "odeou.h"
+
+
+/*extern */
+void dJointSetAMotorNumAxes(dJointID j, int num)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(num, dSA__MIN, dSA__MAX + 1));
+ checktype(joint, AMotor);
+
+ num = dCLAMP(num, dSA__MIN, dSA__MAX);
+
+ joint->setNumAxes(num);
+}
+
+/*extern */
+void dJointSetAMotorAxis(dJointID j, int anum, int rel/*=dJointBodyRelativity*/,
+ dReal x, dReal y, dReal z)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ dAASSERT(dIN_RANGE(rel, dJBR__MIN, dJBR__MAX));
+ checktype(joint, AMotor);
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ joint->setAxisValue(anum, (dJointBodyRelativity)rel, x, y, z);
+}
+
+/*extern */
+void dJointSetAMotorAngle(dJointID j, int anum, dReal angle)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ checktype(joint, AMotor);
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ joint->setAngleValue(anum, angle);
+}
+
+/*extern */
+void dJointSetAMotorParam(dJointID j, int parameter, dReal value)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ checktype(joint, AMotor);
+
+ int anum = parameter >> 8;
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ int limotParam = parameter & 0xff;
+ joint->setLimotParameter(anum, limotParam, value);
+}
+
+/*extern */
+void dJointSetAMotorMode(dJointID j, int mode)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ checktype(joint, AMotor);
+
+ joint->setOperationMode(mode);
+}
+
+/*extern */
+int dJointGetAMotorNumAxes(dJointID j)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ checktype(joint, AMotor);
+
+ return joint->getNumAxes();
+}
+
+/*extern */
+void dJointGetAMotorAxis(dJointID j, int anum, dVector3 result)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ checktype(joint, AMotor);
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ joint->getAxisValue(result, anum);
+}
+
+/*extern */
+int dJointGetAMotorAxisRel(dJointID j, int anum)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ checktype(joint, AMotor);
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ int result = joint->getAxisBodyRelativity(anum);
+ return result;
+}
+
+/*extern */
+dReal dJointGetAMotorAngle(dJointID j, int anum)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ checktype(joint, AMotor);
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ dReal result = joint->getAngleValue(anum);
+ return result;
+}
+
+/*extern */
+dReal dJointGetAMotorAngleRate(dJointID j, int anum)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ checktype(joint, AMotor);
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ dReal result = joint->calculateAngleRate(anum);
+ return result;
+}
+
+/*extern */
+dReal dJointGetAMotorParam(dJointID j, int parameter)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ checktype(joint, AMotor);
+
+ int anum = parameter >> 8;
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+ anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+ int limotParam = parameter & 0xff;
+ dReal result = joint->getLimotParameter(anum, limotParam);
+ return result;
+}
+
+/*extern */
+int dJointGetAMotorMode(dJointID j)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ checktype(joint, AMotor);
+
+ int result = joint->getOperationMode();
+ return result;
+}
+
+/*extern */
+void dJointAddAMotorTorques(dJointID j, dReal torque1, dReal torque2, dReal torque3)
+{
+ dxJointAMotor* joint = (dxJointAMotor*)j;
+ dAASSERT(joint != NULL);
+ checktype(joint, AMotor);
+
+ joint->addTorques(torque1, torque2, torque3);
+}
+
+
+//****************************************************************************
+
+BEGIN_NAMESPACE_OU();
+template<>
+const dJointBodyRelativity CEnumUnsortedElementArray<dSpaceAxis, dSA__MAX, dJointBodyRelativity, 0x160703D5>::m_aetElementArray[] =
+{
+ dJBR_BODY1, // dSA_X,
+ dJBR_GLOBAL, // dSA_Y,
+ dJBR_BODY2, // dSA_Z,
+};
+END_NAMESPACE_OU();
+static const CEnumUnsortedElementArray<dSpaceAxis, dSA__MAX, dJointBodyRelativity, 0x160703D5> g_abrEulerAxisAllowedBodyRelativities;
+
+static inline
+dSpaceAxis EncodeJointConnectedBodyEulerAxis(dJointConnectedBody cbBodyIndex)
+{
+ dSASSERT(dJCB__MAX == 2);
+
+ return cbBodyIndex == dJCB_FIRST_BODY ? dSA_X : dSA_Z;
+}
+
+static inline
+dSpaceAxis EncodeOtherEulerAxis(dSpaceAxis saOneAxis)
+{
+ dIASSERT(saOneAxis == EncodeJointConnectedBodyEulerAxis(dJCB_FIRST_BODY) || saOneAxis == EncodeJointConnectedBodyEulerAxis(dJCB_SECOND_BODY));
+ dSASSERT(dJCB__MAX == 2);
+
+ return (dSpaceAxis)(dSA_X + dSA_Z - saOneAxis);
+}
+
+
+//****************************************************************************
+// angular motor
+
+dxJointAMotor::dxJointAMotor(dxWorld *w) :
+ dxJointAMotor_Parent(w),
+ m_mode(dAMotorUser),
+ m_num(0)
+{
+ std::fill(m_rel, m_rel + dARRAY_SIZE(m_rel), dJBR__DEFAULT);
+ { for (int i = 0; i != dARRAY_SIZE(m_axis); ++i) { dZeroVector3(m_axis[i]); } }
+ { for (int i = 0; i != dARRAY_SIZE(m_references); ++i) { dZeroVector3(m_references[i]); } }
+ std::fill(m_angle, m_angle + dARRAY_SIZE(m_angle), REAL(0.0));
+ { for (int i = 0; i != dARRAY_SIZE(m_limot); ++i) { m_limot[i].init(w); } }
+}
+
+
+/*virtual */
+dxJointAMotor::~dxJointAMotor()
+{
+ // The virtual destructor
+}
+
+
+/*virtual */
+void dxJointAMotor::getSureMaxInfo(SureMaxInfo* info)
+{
+ info->max_m = m_num;
+}
+
+/*virtual */
+void dxJointAMotor::getInfo1(dxJoint::Info1 *info)
+{
+ info->m = 0;
+ info->nub = 0;
+
+ // compute the axes and angles, if in Euler mode
+ if (m_mode == dAMotorEuler)
+ {
+ dVector3 ax[dSA__MAX];
+ computeGlobalAxes(ax);
+ computeEulerAngles(ax);
+ }
+
+ // see if we're powered or at a joint limit for each axis
+ const unsigned num = m_num;
+ for (unsigned i = 0; i != num; ++i)
+ {
+ if (m_limot[i].testRotationalLimit(m_angle[i])
+ || m_limot[i].fmax > 0)
+ {
+ info->m++;
+ }
+ }
+}
+
+/*virtual */
+void dxJointAMotor::getInfo2(dReal worldFPS, dReal /*worldERP*/,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex)
+{
+ // compute the axes (if not global)
+ dVector3 ax[dSA__MAX];
+ computeGlobalAxes(ax);
+
+ // in Euler angle mode we do not actually constrain the angular velocity
+ // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+ //
+ // to get constrain w2-w1 along ...not
+ // ------ --------------------- ------
+ // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
+ // d(angle[1])/dt = 0 ax[1]
+ // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
+ //
+ // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+ // to prove the result for angle[0], write the expression for angle[0] from
+ // GetInfo1 then take the derivative. to prove this for angle[2] it is
+ // easier to take the Euler rate expression for d(angle[2])/dt with respect
+ // to the components of w and set that to 0.
+
+ dVector3 *axptr[dSA__MAX];
+ for (int j = dSA__MIN; j != dSA__MAX; ++j) { axptr[j] = &ax[j]; }
+
+ dVector3 ax0_cross_ax1;
+ dVector3 ax1_cross_ax2;
+
+ if (m_mode == dAMotorEuler)
+ {
+ dCalcVectorCross3(ax0_cross_ax1, ax[dSA_X], ax[dSA_Y]);
+ axptr[dSA_Z] = &ax0_cross_ax1;
+ dCalcVectorCross3(ax1_cross_ax2, ax[dSA_Y], ax[dSA_Z]);
+ axptr[dSA_X] = &ax1_cross_ax2;
+ }
+
+ sizeint rowTotalSkip = 0, pairTotalSkip = 0;
+
+ const unsigned num = m_num;
+ for (unsigned i = 0; i != num; ++i)
+ {
+ if (m_limot[i].addLimot(this, worldFPS, J1 + rowTotalSkip, J2 + rowTotalSkip, pairRhsCfm + pairTotalSkip, pairLoHi + pairTotalSkip, *(axptr[i]), 1))
+ {
+ rowTotalSkip += rowskip;
+ pairTotalSkip += pairskip;
+ }
+ }
+}
+
+/*virtual */
+dJointType dxJointAMotor::type() const
+{
+ return dJointTypeAMotor;
+}
+
+/*virtual */
+sizeint dxJointAMotor::size() const
+{
+ return sizeof(*this);
+}
+
+
+void dxJointAMotor::setOperationMode(int mode)
+{
+ m_mode = mode;
+
+ if (mode == dAMotorEuler)
+ {
+ m_num = dSA__MAX;
+ setEulerReferenceVectors();
+ }
+}
+
+
+void dxJointAMotor::setNumAxes(unsigned num)
+{
+ if (m_mode == dAMotorEuler)
+ {
+ m_num = dSA__MAX;
+ }
+ else
+ {
+ m_num = num;
+ }
+}
+
+
+dJointBodyRelativity dxJointAMotor::getAxisBodyRelativity(unsigned anum) const
+{
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+ dJointBodyRelativity rel = m_rel[anum];
+ if (dJBREncodeBodyRelativityStatus(rel) && GetIsJointReverse())
+ {
+ rel = dJBRSwapBodyRelativity(rel); // turns 1 into 2, 2 into 1
+ }
+
+ return rel;
+}
+
+
+void dxJointAMotor::setAxisValue(unsigned anum, dJointBodyRelativity rel,
+ dReal x, dReal y, dReal z)
+{
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ dAASSERT(m_mode != dAMotorEuler || !dJBREncodeBodyRelativityStatus(rel) || rel == g_abrEulerAxisAllowedBodyRelativities.Encode((dSpaceAxis)anum));
+
+ // x,y,z is always in global coordinates regardless of rel, so we may have
+ // to convert it to be relative to a body
+ dVector3 r;
+ dAssignVector3(r, x, y, z);
+
+ // adjust rel to match the internal body order
+ if (dJBREncodeBodyRelativityStatus(rel) && GetIsJointReverse())
+ {
+ rel = dJBRSwapBodyRelativity(rel); // turns 1 into 2, 2, into 1
+ }
+
+ m_rel[anum] = rel;
+
+ bool assigned = false;
+
+ if (dJBREncodeBodyRelativityStatus(rel))
+ {
+ if (rel == dJBR_BODY1)
+ {
+ dMultiply1_331(m_axis[anum], this->node[0].body->posr.R, r);
+ assigned = true;
+ }
+ // rel == 2
+ else if (this->node[1].body != NULL)
+ {
+ dIASSERT(rel == dJBR_BODY2);
+
+ dMultiply1_331(m_axis[anum], this->node[1].body->posr.R, r);
+ assigned = true;
+ }
+ }
+
+ if (!assigned)
+ {
+ dCopyVector3(m_axis[anum], r);
+ }
+
+ dNormalize3(m_axis[anum]);
+
+ if (m_mode == dAMotorEuler)
+ {
+ setEulerReferenceVectors();
+ }
+}
+
+void dxJointAMotor::getAxisValue(dVector3 result, unsigned anum) const
+{
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+ switch (m_mode)
+ {
+ case dAMotorUser:
+ {
+ doGetUserAxis(result, anum);
+ break;
+ }
+
+ case dAMotorEuler:
+ {
+ doGetEulerAxis(result, anum);
+ break;
+ }
+
+ default:
+ {
+ dIASSERT(false);
+ break;
+ }
+ }
+}
+
+
+void dxJointAMotor::doGetUserAxis(dVector3 result, unsigned anum) const
+{
+ bool retrieved = false;
+
+ if (dJBREncodeBodyRelativityStatus(m_rel[anum]))
+ {
+ if (m_rel[anum] == dJBR_BODY1)
+ {
+ dMultiply0_331(result, this->node[0].body->posr.R, m_axis[anum]);
+ retrieved = true;
+ }
+ else if (this->node[1].body != NULL)
+ {
+ dMultiply0_331(result, this->node[1].body->posr.R, m_axis[anum]);
+ retrieved = true;
+ }
+ }
+
+ if (!retrieved)
+ {
+ dCopyVector3(result, m_axis[anum]);
+ }
+}
+
+void dxJointAMotor::doGetEulerAxis(dVector3 result, unsigned anum) const
+{
+ // If we're in Euler mode, joint->axis[1] doesn't
+ // have anything sensible in it. So don't just return
+ // that, find the actual effective axis.
+ // Likewise, the actual axis of rotation for the
+ // the other axes is different from what's stored.
+ dVector3 axes[dSA__MAX];
+ computeGlobalAxes(axes);
+
+ if (anum == dSA_Y)
+ {
+ dCopyVector3(result, axes[dSA_Y]);
+ }
+ else if (anum < dSA_Y) // Comparing against the same constant lets compiler reuse EFLAGS register for another conditional jump
+ {
+ dSASSERT(dSA_X < dSA_Y); // Otherwise the condition above is incorrect
+ dIASSERT(anum == dSA_X);
+
+ // This won't be unit length in general,
+ // but it's what's used in getInfo2
+ // This may be why things freak out as
+ // the body-relative axes get close to each other.
+ dCalcVectorCross3(result, axes[dSA_Y], axes[dSA_Z]);
+ }
+ else
+ {
+ dSASSERT(dSA_Z > dSA_Y); // Otherwise the condition above is incorrect
+ dIASSERT(anum == dSA_Z);
+
+ // Same problem as above.
+ dCalcVectorCross3(result, axes[dSA_X], axes[dSA_Y]);
+ }
+}
+
+
+void dxJointAMotor::setAngleValue(unsigned anum, dReal angle)
+{
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ dAASSERT(m_mode == dAMotorUser); // This only works for the dAMotorUser
+
+ if (m_mode == dAMotorUser)
+ {
+ m_angle[anum] = angle;
+ }
+}
+
+
+dReal dxJointAMotor::calculateAngleRate(unsigned anum) const
+{
+ dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+ dAASSERT(this->node[0].body != NULL); // Don't call for angle rate before the joint is set up
+
+ dVector3 axis;
+ getAxisValue(axis, anum);
+
+ // NOTE!
+ // For reverse joints, the rate is negated at the function exit to create swapped bodies effect
+ dReal rate = dDOT(axis, this->node[0].body->avel);
+
+ if (this->node[1].body != NULL)
+ {
+ rate -= dDOT(axis, this->node[1].body->avel);
+ }
+
+ // Negating the rate for reverse joints creates an effect of body swapping
+ dReal result = !GetIsJointReverse() ? rate : -rate;
+ return result;
+}
+
+
+void dxJointAMotor::addTorques(dReal torque1, dReal torque2, dReal torque3)
+{
+ unsigned num = getNumAxes();
+ dAASSERT(dIN_RANGE(num, dSA__MIN, dSA__MAX + 1));
+
+ dVector3 sum;
+ dVector3 torqueVector;
+ dVector3 axes[dSA__MAX];
+
+
+ if (num != dSA__MIN)
+ {
+ computeGlobalAxes(axes);
+
+ if (!GetIsJointReverse())
+ {
+ dAssignVector3(torqueVector, torque1, torque2, torque3);
+ }
+ else
+ {
+ // Negating torques creates an effect of swapped bodies later
+ dAssignVector3(torqueVector, -torque1, -torque2, -torque3);
+ }
+ }
+
+ switch (num)
+ {
+ case dSA_Z + 1:
+ {
+ dAddThreeScaledVectors3(sum, axes[dSA_Z], axes[dSA_Y], axes[dSA_X], torqueVector[dSA_Z], torqueVector[dSA_Y], torqueVector[dSA_X]);
+ break;
+ }
+
+ case dSA_Y + 1:
+ {
+ dAddScaledVectors3(sum, axes[dSA_Y], axes[dSA_X], torqueVector[dSA_Y], torqueVector[dSA_X]);
+ break;
+ }
+
+ case dSA_X + 1:
+ {
+ dCopyScaledVector3(sum, axes[dSA_X], torqueVector[dSA_X]);
+ break;
+ }
+
+ default:
+ {
+ dSASSERT(dSA_Z > dSA_Y); // Otherwise the addends order needs to be switched
+ dSASSERT(dSA_Y > dSA_X);
+
+ // Do nothing
+ break;
+ }
+ }
+
+ if (num != dSA__MIN)
+ {
+ dAASSERT(this->node[0].body != NULL); // Don't add torques unless you set the joint up first!
+
+ // NOTE!
+ // For reverse joints, the torqueVector negated at function entry produces the effect of swapped bodies
+ dBodyAddTorque(this->node[0].body, sum[dV3E_X], sum[dV3E_Y], sum[dV3E_Z]);
+
+ if (this->node[1].body != NULL)
+ {
+ dBodyAddTorque(this->node[1].body, -sum[dV3E_X], -sum[dV3E_Y], -sum[dV3E_Z]);
+ }
+ }
+}
+
+
+// compute the 3 axes in global coordinates
+void dxJointAMotor::computeGlobalAxes(dVector3 ax[dSA__MAX]) const
+{
+ switch (m_mode)
+ {
+ case dAMotorUser:
+ {
+ doComputeGlobalUserAxes(ax);
+ break;
+ }
+
+ case dAMotorEuler:
+ {
+ doComputeGlobalEulerAxes(ax);
+ break;
+ }
+
+ default:
+ {
+ dIASSERT(false);
+ break;
+ }
+ }
+}
+
+void dxJointAMotor::doComputeGlobalUserAxes(dVector3 ax[dSA__MAX]) const
+{
+ unsigned num = m_num;
+ for (unsigned i = 0; i != num; ++i)
+ {
+ bool assigned = false;
+
+ if (m_rel[i] == dJBR_BODY1)
+ {
+ // relative to b1
+ dMultiply0_331(ax[i], this->node[0].body->posr.R, m_axis[i]);
+ assigned = true;
+ }
+ else if (m_rel[i] == dJBR_BODY2)
+ {
+ // relative to b2
+ if (this->node[1].body != NULL)
+ {
+ dMultiply0_331(ax[i], this->node[1].body->posr.R, m_axis[i]);
+ assigned = true;
+ }
+ }
+
+ if (!assigned)
+ {
+ // global - just copy it
+ dCopyVector3(ax[i], m_axis[i]);
+ }
+ }
+}
+
+void dxJointAMotor::doComputeGlobalEulerAxes(dVector3 ax[dSA__MAX]) const
+{
+ // special handling for Euler mode
+
+ dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis();
+ dMultiply0_331(ax[firstBodyAxis], this->node[0].body->posr.R, m_axis[firstBodyAxis]);
+
+ dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis);
+
+ if (this->node[1].body != NULL)
+ {
+ dMultiply0_331(ax[secondBodyAxis], this->node[1].body->posr.R, m_axis[secondBodyAxis]);
+ }
+ else
+ {
+ dCopyVector3(ax[secondBodyAxis], m_axis[secondBodyAxis]);
+ }
+
+ dCalcVectorCross3(ax[dSA_Y], ax[dSA_Z], ax[dSA_X]);
+ dNormalize3(ax[dSA_Y]);
+}
+
+
+void dxJointAMotor::computeEulerAngles(dVector3 ax[dSA__MAX])
+{
+ // assumptions:
+ // global axes already calculated --> ax
+ // axis[0] is relative to body 1 --> global ax[0]
+ // axis[2] is relative to body 2 --> global ax[2]
+ // ax[1] = ax[2] x ax[0]
+ // original ax[0] and ax[2] are perpendicular
+ // reference1 is perpendicular to ax[0] (in body 1 frame)
+ // reference2 is perpendicular to ax[2] (in body 2 frame)
+ // all ax[] and reference vectors are unit length
+
+ // calculate references in global frame
+ dVector3 refs[dJCB__MAX];
+ dMultiply0_331(refs[dJCB_FIRST_BODY], this->node[0].body->posr.R, m_references[dJCB_FIRST_BODY]);
+
+ if (this->node[1].body != NULL)
+ {
+ dMultiply0_331(refs[dJCB_SECOND_BODY], this->node[1].body->posr.R, m_references[dJCB_SECOND_BODY]);
+ }
+ else
+ {
+ dCopyVector3(refs[dJCB_SECOND_BODY], m_references[dJCB_SECOND_BODY]);
+ }
+
+
+ // get q perpendicular to both ax[0] and ref1, get first euler angle
+ dVector3 q;
+ dJointConnectedBody firstAxisBody = BuildFirstEulerAxisBody();
+
+ dCalcVectorCross3(q, ax[dSA_X], refs[firstAxisBody]);
+ m_angle[dSA_X] = -dAtan2(dCalcVectorDot3(ax[dSA_Z], q), dCalcVectorDot3(ax[dSA_Z], refs[firstAxisBody]));
+
+ // get q perpendicular to both ax[0] and ax[1], get second euler angle
+ dCalcVectorCross3(q, ax[dSA_X], ax[dSA_Y]);
+ m_angle[dSA_Y] = -dAtan2(dCalcVectorDot3(ax[dSA_Z], ax[dSA_X]), dCalcVectorDot3(ax[dSA_Z], q));
+
+ dJointConnectedBody secondAxisBody = EncodeJointOtherConnectedBody(firstAxisBody);
+
+ // get q perpendicular to both ax[1] and ax[2], get third euler angle
+ dCalcVectorCross3(q, ax[dSA_Y], ax[dSA_Z]);
+ m_angle[dSA_Z] = -dAtan2(dCalcVectorDot3(refs[secondAxisBody], ax[dSA_Y]), dCalcVectorDot3(refs[secondAxisBody], q));
+}
+
+
+// set the reference vectors as follows:
+// * reference1 = current axis[2] relative to body 1
+// * reference2 = current axis[0] relative to body 2
+// this assumes that:
+// * axis[0] is relative to body 1
+// * axis[2] is relative to body 2
+
+void dxJointAMotor::setEulerReferenceVectors()
+{
+ if (/*this->node[0].body != NULL && */this->node[1].body != NULL)
+ {
+ dIASSERT(this->node[0].body != NULL);
+
+ dVector3 r; // axis[2] and axis[0] in global coordinates
+
+ dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis();
+ dMultiply0_331(r, this->node[0].body->posr.R, m_axis[firstBodyAxis]);
+ dMultiply1_331(m_references[dJCB_SECOND_BODY], this->node[1].body->posr.R, r);
+
+ dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis);
+ dMultiply0_331(r, this->node[1].body->posr.R, m_axis[secondBodyAxis]);
+ dMultiply1_331(m_references[dJCB_FIRST_BODY], this->node[0].body->posr.R, r);
+ }
+ else
+ {
+ // We want to handle angular motors attached to passive geoms
+ // Replace missing node.R with identity
+ if (this->node[0].body != NULL)
+ {
+ dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis();
+ dMultiply0_331(m_references[dJCB_SECOND_BODY], this->node[0].body->posr.R, m_axis[firstBodyAxis]);
+
+ dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis);
+ dMultiply1_331(m_references[dJCB_FIRST_BODY], this->node[0].body->posr.R, m_axis[secondBodyAxis]);
+ }
+ }
+}
+
+/*inline */
+dSpaceAxis dxJointAMotor::BuildFirstBodyEulerAxis() const
+{
+ return EncodeJointConnectedBodyEulerAxis(BuildFirstEulerAxisBody());
+}
+
+/*inline */
+dJointConnectedBody dxJointAMotor::BuildFirstEulerAxisBody() const
+{
+ return !GetIsJointReverse() ? dJCB_FIRST_BODY : dJCB_SECOND_BODY;
+}
+
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 00:17:51 +0000