|
From: <t_l...@us...> - 2008-06-15 15:34:22
|
Revision: 586
http://joode.svn.sourceforge.net/joode/?rev=586&view=rev
Author: t_larkworthy
Date: 2008-06-15 08:34:17 -0700 (Sun, 15 Jun 2008)
Log Message:
-----------
Translated ODE JointAMotor code to JOODE. Will write test soon.
Added Paths:
-----------
trunk/src/java/net/java/dev/joode/joint/JointAMotor.java
Added: trunk/src/java/net/java/dev/joode/joint/JointAMotor.java
===================================================================
--- trunk/src/java/net/java/dev/joode/joint/JointAMotor.java (rev 0)
+++ trunk/src/java/net/java/dev/joode/joint/JointAMotor.java 2008-06-15 15:34:17 UTC (rev 586)
@@ -0,0 +1,861 @@
+package net.java.dev.joode.joint;
+
+import net.java.dev.joode.SimState;
+import net.java.dev.joode.ClonedReferences;
+import net.java.dev.joode.World;
+import net.java.dev.joode.util.Vector3;
+import net.java.dev.joode.util.MathUtils;
+
+/**
+ */
+public class JointAMotor extends Joint{
+
+ int num; // number of axes (0..3)
+ int mode; // a dAMotorXXX constant
+ int [] rel = new int[3]; // what the axes are relative to (global,b1,b2)
+ Vector3 [] axis = new Vector3[3]; // three axes
+ JointLimitMotor [] limot = new JointLimitMotor[3]; // limit+motor info for axes
+ Vector3 angle; // user-supplied angles for axes
+ // these vectors are used for calculating euler angles
+ Vector3 reference1; // original axis[2], relative to body 1
+ Vector3 reference2; // original axis[0], relative to body 2
+
+
+ public static final int MODE_USER=1;
+ public static final int MODE_EULER=0;
+
+ public JointAMotor(World world) {
+ super(world);
+ int i;
+ JointAMotor j = this;
+
+ j.num = 0;
+ j.mode = MODE_USER;
+ for (i=0; i<3; i++) {
+ j.rel[i] = 0;
+ j.axis[i].setZero();
+ j.limot[i] = new JointLimitMotor (world);
+ j.angle.m[i] = 0;
+ }
+ j.reference1.setZero();
+ j.reference2.setZero();
+ }
+
+ void amotorComputeGlobalAxes (Vector3 [] ax)
+
+ {
+ JointAMotor joint = this;
+ if (joint.mode == MODE_EULER) {
+ // special handling for euler mode
+ MathUtils.dMULTIPLY0_331 (ax[0],joint.getBody(0).getRotation(),joint.axis[0]);
+ if (joint.getBody(1) != null) {
+ MathUtils.dMULTIPLY0_331 (ax[2],joint.getBody(1).getRotation(),joint.axis[2]);
+ }
+ else {
+ ax[2].m[0] = joint.axis[2].m[0];
+ ax[2].m[1] = joint.axis[2].m[1];
+ ax[2].m[2] = joint.axis[2].m[2];
+ }
+ ax[2].cross(ax[0], ax[1]);
+
+ ax[1].normalize();
+ }
+ else {
+ for (int i=0; i < joint.num; i++) {
+ if (joint.rel[i] == 1) {
+ // relative to b1
+ MathUtils.dMULTIPLY0_331 (ax[i],joint.getBody(0).getRotation(),joint.axis[i]);
+ }
+ else if (joint.rel[i] == 2) {
+ // relative to b2
+ if (joint.getBody(1)!=null) { // jds: don't assert, just ignore
+ MathUtils.dMULTIPLY0_331 (ax[i],joint.getBody(1).getRotation(),joint.axis[i]);
+ }
+ }
+ else {
+ // global - just copy it
+ ax[i].m[0] = joint.axis[i].m[0];
+ ax[i].m[1] = joint.axis[i].m[1];
+ ax[i].m[2] = joint.axis[i].m[2];
+ }
+ }
+ }
+ }
+
+ void amotorComputeEulerAngles (Vector3 []ax)
+ {
+ JointAMotor joint = this;
+ // 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
+ Vector3 ref1 = new Vector3(),ref2=new Vector3();
+ MathUtils.dMULTIPLY0_331 (ref1,joint.getBody(0).getRotation(),joint.reference1);
+ if (joint.getBody(1)!=null) {
+ MathUtils.dMULTIPLY0_331 (ref2,joint.getBody(1).getRotation(),joint.reference2);
+ }
+ else {
+ ref2.m[0] = joint.reference2.m[0];
+ ref2.m[1] = joint.reference2.m[1];
+ ref2.m[2] = joint.reference2.m[2];
+ }
+
+ // get q perpendicular to both ax[0] and ref1, get first euler angle
+ Vector3 q = new Vector3();
+ ax[0].cross(ref1, q);
+
+ joint.angle.m[0] = (float) -Math.atan2 (ax[2].dot(q),ax[2].dot(ref1));
+
+ // get q perpendicular to both ax[0] and ax[1], get second euler angle
+ ax[0].cross(ax[1], q);
+ joint.angle.m[1] = (float) -Math.atan2 (ax[2].dot(ax[0]),ax[2].dot(q));
+
+ // get q perpendicular to both ax[1] and ax[2], get third euler angle
+ ax[1].cross(ax[2], q);
+ joint.angle.m[2] =(float) -Math.atan2 (ref2.dot(ax[1]),ref2.dot(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 amotorSetEulerReferenceVectors ()
+ {
+ JointAMotor j = this;
+ if (j.getBody(0)!=null && j.getBody(1)!=null) {
+ Vector3 r = new Vector3(); // axis[2] and axis[0] in global coordinates
+ MathUtils.dMULTIPLY0_331 (r,j.getBody(1).getRotation(),j.axis[2]);
+ MathUtils.dMULTIPLY1_331 (j.reference1,j.getBody(0).getRotation(),r);
+ MathUtils.dMULTIPLY0_331 (r,j.getBody(0).getRotation(),j.axis[0]);
+ MathUtils.dMULTIPLY1_331 (j.reference2,j.getBody(1).getRotation(),r);
+ }
+
+ else { // jds
+ // else if (j.node[0].body) {
+ // dMULTIPLY1_331 (j.reference1,j.node[0].body.R,j.axis[2]);
+ // dMULTIPLY0_331 (j.reference2,j.node[0].body.R,j.axis[0]);
+
+ // We want to handle angular motors attached to passive geoms
+ Vector3 r = new Vector3(); // axis[2] and axis[0] in global coordinates
+ r.m[0] = j.axis[2].m[0];
+ r.m[1] = j.axis[2].m[1];
+ r.m[2] = j.axis[2].m[2];
+ r.m[3] = j.axis[2].m[3];
+ MathUtils.dMULTIPLY1_331 (j.reference1,j.getBody(0).getRotation(),r);
+ MathUtils.dMULTIPLY0_331 (r,j.getBody(0).getRotation(),j.axis[0]);
+ j.reference2.m[0] += r.m[0];
+ j.reference2.m[1] += r.m[1];
+ j.reference2.m[2] += r.m[2];
+ j.reference2.m[3] += r.m[3];
+ }
+ }
+
+
+ public void getInfo1(Info1 info) {
+ info.m = 0;
+ info.nub = 0;
+
+ // compute the axes and angles, if in euler mode
+ if (this.mode == MODE_EULER) {
+ Vector3 []ax = new Vector3[3];
+ this.amotorComputeGlobalAxes (ax);
+ this.amotorComputeEulerAngles (ax);
+ }
+
+ // see if we're powered or at a joint limit for each axis
+ for (int i=0; i < this.num; i++) {
+ if (this.limot[i].testRotationalLimit (this.angle.m[i]) ||
+ this.limot[i].fmax > 0) {
+ info.m++;
+ }
+ }
+ }
+
+ public void getInfo2(Info2 info) {
+ int i;
+ JointAMotor joint = this;
+ // compute the axes (if not global)
+ Vector3 [] ax = new Vector3[3];
+ this.amotorComputeGlobalAxes (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.
+
+ Vector3 [] axptr = new Vector3[3];
+ axptr[0] = ax[0];
+ axptr[1] = ax[1];
+ axptr[2] = ax[2];
+
+ Vector3 ax0_cross_ax1 = new Vector3();
+ Vector3 ax1_cross_ax2 = new Vector3();
+ if (joint.mode == MODE_EULER) {
+ ax[0].cross(ax[2], ax0_cross_ax1);
+ axptr[2] = ax0_cross_ax1;
+ ax[1].cross(ax[2], ax1_cross_ax2);
+ axptr[0] = ax1_cross_ax2;
+ }
+
+ int row=0;
+ for (i=0; i < joint.num; i++) {
+ if(joint.limot[i].addLimot (joint,info,row,axptr[i],true))
+ row ++;
+ }
+ }
+
+ public SimState cloneState(ClonedReferences util) {
+ return null;
+ }
+
+void dJointSetAMotorNumAxes (int num)
+{
+ JointAMotor joint = this;
+ assert(num >= 0 && num <= 3);
+ if (joint.mode == MODE_EULER) {
+ joint.num = 3;
+ }
+ else {
+ if (num < 0) num = 0;
+ if (num > 3) num = 3;
+ joint.num = num;
+ }
+}
+
+
+void dJointSetAMotorAxis (int anum, int rel,
+ float x, float y, float z)
+{
+ JointAMotor joint = this;
+ assert(anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
+ assert(!(joint.getBody(1)==null&& ((joint.flags & Joint.FLAG_JOINT_REVERSE) != 0) && rel == 1)):"no first body, can't set axis rel=1";
+ assert(!(joint.getBody(1)==null && !((joint.flags & Joint.FLAG_JOINT_REVERSE) != 0) && rel == 2)):"no second body, can't set axis rel=2";
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+
+ // adjust rel to match the internal body order
+ if (joint.getBody(1)==null && rel==2) rel = 1;
+
+ joint.rel[anum] = rel;
+
+ // x,y,z is always in global coordinates regardless of rel, so we may have
+ // to convert it to be relative to a body
+ Vector3 r = new Vector3();
+ r.m[0] = x;
+ r.m[1] = y;
+ r.m[2] = z;
+ r.m[3] = 0;
+ if (rel > 0) {
+ if (rel==1) {
+ MathUtils.dMULTIPLY1_331 (joint.axis[anum],joint.getBody(0).getRotation(),r);
+ }
+ else {
+ // don't assert; handle the case of attachment to a bodiless geom
+ if (joint.getBody(1)!=null) { // jds
+ MathUtils.dMULTIPLY1_331(joint.axis[anum],joint.getBody(1).getRotation(),r);
+ }
+ else {
+ joint.axis[anum].m[0] = r.m[0]; joint.axis[anum].m[1] = r.m[1];
+ joint.axis[anum].m[2] = r.m[2]; joint.axis[anum].m[3] = r.m[3];
+ }
+ }
+ }
+ else {
+ joint.axis[anum].m[0] = r.m[0];
+ joint.axis[anum].m[1] = r.m[1];
+ joint.axis[anum].m[2] = r.m[2];
+ }
+ joint.axis[anum].normalize();
+ if (joint.mode == MODE_EULER) amotorSetEulerReferenceVectors ();
+}
+
+
+void dJointSetAMotorAngle (int anum,
+ float angle)
+{
+ JointAMotor joint = this;
+ assert(anum >= 0 && anum < 3);
+ if (joint.mode == MODE_USER) {
+ if (anum < 0) anum = 0;
+ if (anum > 3) anum = 3;
+ joint.angle.m[anum] = angle;
+ }
+}
+
+
+ /*
+void dJointSetAMotorParam (int parameter,
+ float value)
+{
+ JointAMotor joint = this;
+
+ int anum = parameter >> 8;
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ parameter &= 0xff;
+ joint.limot[anum].setBounce (parameter, value);
+} */
+
+
+void dJointSetAMotorMode (int mode)
+{
+ JointAMotor joint = this;
+
+ joint.mode = mode;
+ if (joint.mode == MODE_EULER) {
+ joint.num = 3;
+ amotorSetEulerReferenceVectors ();
+ }
+}
+
+
+int dJointGetAMotorNumAxes ()
+{
+ JointAMotor joint = this;
+
+ return joint.num;
+}
+
+
+void dJointGetAMotorAxis (int anum,
+ Vector3 result)
+{
+ JointAMotor joint = this;
+
+ assert(anum >= 0 && anum < 3);
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ if (joint.rel[anum] > 0) {
+ if (joint.rel[anum]==1) {
+ MathUtils.dMULTIPLY0_331 (result,joint.getBody(0).getRotation(),joint.axis[anum]);
+ }
+ else {
+ if (joint.getBody(1)!=null) { // jds
+ MathUtils.dMULTIPLY0_331 (result,joint.getBody(1).getRotation(),joint.axis[anum]);
+ }
+ else {
+ result.m[0] = joint.axis[anum].m[0]; result.m[1] = joint.axis[anum].m[1];
+ result.m[2] = joint.axis[anum].m[2]; result.m[3] = joint.axis[anum].m[3];
+ }
+ }
+ }
+ else {
+ result.m[0] = joint.axis[anum].m[0];
+ result.m[1] = joint.axis[anum].m[1];
+ result.m[2] = joint.axis[anum].m[2];
+ }
+}
+
+
+int dJointGetAMotorAxisRel (int anum)
+{
+ JointAMotor joint = this;
+
+ assert(anum >= 0 && anum < 3);
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ return joint.rel[anum];
+}
+
+
+float dJointGetAMotorAngle (int anum)
+{
+ JointAMotor joint = this;
+
+ assert(anum >= 0 && anum < 3);
+ if (anum < 0) anum = 0;
+ if (anum > 3) anum = 3;
+ return joint.angle.m[anum];
+}
+
+
+/*
+float dJointGetAMotorParam (int parameter)
+{
+ JointAMotor joint = this;
+
+ int anum = parameter >> 8;
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ parameter &= 0xff;
+ return joint.limot[anum].get (parameter);
+} */
+
+
+void dJointAddAMotorTorques (float torque1, float torque2, float torque3)
+{
+ JointAMotor joint = this;
+
+ Vector3 [] axes = new Vector3[3];
+
+ if (joint.num == 0)
+ return;
+ assert((joint.flags & Joint.FLAG_JOINT_REVERSE) ==0 ): "dJointAddAMotorTorques not yet implemented for reverse AMotor joints";
+
+ amotorComputeGlobalAxes (axes);
+ axes[0].m[0] *= torque1;
+ axes[0].m[1] *= torque1;
+ axes[0].m[2] *= torque1;
+ if (joint.num >= 2) {
+ axes[0].m[0] += axes[1].m[0] * torque2;
+ axes[0].m[1] += axes[1].m[1] * torque2;
+ axes[0].m[2] += axes[1].m[2] * torque2;
+ if (joint.num >= 3) {
+ axes[0].m[0] += axes[2].m[0] * torque3;
+ axes[0].m[1] += axes[2].m[1] * torque3;
+ axes[0].m[2] += axes[2].m[2] * torque3;
+ }
+ }
+
+ if (joint.getBody(0) != null)
+ joint.getBody(0).addTorque (axes[0].m[0],axes[0].m[1],axes[0].m[2]);
+ if (joint.getBody(1) != null)
+ joint.getBody(1).addTorque (-axes[0].m[0], -axes[0].m[1], -axes[0].m[2]);
+}
+}
+
+ //****************************************************************************
+// angular motor
+
+
+ /*
+static void amotorInit (dxJointAMotor *j)
+{
+ int i;
+ j->num = 0;
+ j->mode = MODE_USER;
+ for (i=0; i<3; i++) {
+ j->rel[i] = 0;
+ dSetZero (j->axis[i],4);
+ j->limot[i].init (j->world);
+ j->angle[i] = 0;
+ }
+ dSetZero (j->reference1,4);
+ dSetZero (j->reference2,4);
+}
+
+
+// compute the 3 axes in global coordinates
+
+static void amotorComputeGlobalAxes (dVector3 ax[3])
+{
+ if (joint->mode == MODE_EULER) {
+ // special handling for euler mode
+ MathUtils.dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]);
+ if (joint->node[1].body) {
+ MathUtils.dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]);
+ }
+ else {
+ ax[2][0] = joint->axis[2][0];
+ ax[2][1] = joint->axis[2][1];
+ ax[2][2] = joint->axis[2][2];
+ }
+ dCROSS (ax[1],=,ax[2],ax[0]);
+ dNormalize3 (ax[1]);
+ }
+ else {
+ for (int i=0; i < joint->num; i++) {
+ if (joint->rel[i] == 1) {
+ // relative to b1
+ MathUtils.dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]);
+ }
+ else if (joint->rel[i] == 2) {
+ // relative to b2
+ if (joint->node[1].body) { // jds: don't assert, just ignore
+ MathUtils.dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]);
+ }
+ }
+ else {
+ // global - just copy it
+ ax[i][0] = joint->axis[i][0];
+ ax[i][1] = joint->axis[i][1];
+ ax[i][2] = joint->axis[i][2];
+ }
+ }
+ }
+}
+
+
+static void amotorComputeEulerAngles (dVector3 ax[3])
+{
+ // 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 ref1,ref2;
+ MathUtils.dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1);
+ if (joint->node[1].body) {
+ MathUtils.dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2);
+ }
+ else {
+ ref2[0] = joint->reference2[0];
+ ref2[1] = joint->reference2[1];
+ ref2[2] = joint->reference2[2];
+ }
+
+ // get q perpendicular to both ax[0] and ref1, get first euler angle
+ dVector3 q;
+ dCROSS (q,=,ax[0],ref1);
+ joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1));
+
+ // get q perpendicular to both ax[0] and ax[1], get second euler angle
+ dCROSS (q,=,ax[0],ax[1]);
+ joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q));
+
+ // get q perpendicular to both ax[1] and ax[2], get third euler angle
+ dCROSS (q,=,ax[1],ax[2]);
+ joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,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
+
+static void amotorSetEulerReferenceVectors (dxJointAMotor *j)
+{
+ if (j->node[0].body && j->node[1].body) {
+ dVector3 r; // axis[2] and axis[0] in global coordinates
+ MathUtils.dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]);
+ dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r);
+ MathUtils.dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]);
+ dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r);
+ }
+
+ else { // jds
+ // else if (j->node[0].body) {
+ // dMULTIPLY1_331 (j->reference1,j->node[0].body->R,j->axis[2]);
+ // MathUtils.dMULTIPLY0_331 (j->reference2,j->node[0].body->R,j->axis[0]);
+
+ // We want to handle angular motors attached to passive geoms
+ dVector3 r; // axis[2] and axis[0] in global coordinates
+ r[0] = j->axis[2][0]; r[1] = j->axis[2][1]; r[2] = j->axis[2][2]; r[3] = j->axis[2][3];
+ dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r);
+ MathUtils.dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]);
+ j->reference2[0] += r[0]; j->reference2[1] += r[1];
+ j->reference2[2] += r[2]; j->reference2[3] += r[3];
+ }
+}
+
+
+static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info)
+{
+ info->m = 0;
+ info->nub = 0;
+
+ // compute the axes and angles, if in euler mode
+ if (j->mode == MODE_EULER) {
+ dVector3 ax[3];
+ amotorComputeGlobalAxes (j,ax);
+ amotorComputeEulerAngles (j,ax);
+ }
+
+ // see if we're powered or at a joint limit for each axis
+ for (int i=0; i < j->num; i++) {
+ if (j->limot[i].testRotationalLimit (j->angle[i]) ||
+ j->limot[i].fmax > 0) {
+ info->m++;
+ }
+ }
+}
+
+
+static void amotorGetInfo2 (dxJoint::Info2 *info)
+{
+ int i;
+
+ // compute the axes (if not global)
+ dVector3 ax[3];
+ amotorComputeGlobalAxes (joint,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[3];
+ axptr[0] = &ax[0];
+ axptr[1] = &ax[1];
+ axptr[2] = &ax[2];
+
+ dVector3 ax0_cross_ax1;
+ dVector3 ax1_cross_ax2;
+ if (joint->mode == MODE_EULER) {
+ dCROSS (ax0_cross_ax1,=,ax[0],ax[1]);
+ axptr[2] = &ax0_cross_ax1;
+ dCROSS (ax1_cross_ax2,=,ax[1],ax[2]);
+ axptr[0] = &ax1_cross_ax2;
+ }
+
+ int row=0;
+ for (i=0; i < joint->num; i++) {
+ row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1);
+ }
+}
+
+
+void dJointSetAMotorNumAxes (int num)
+{
+ assert(joint && num >= 0 && num <= 3);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ if (joint->mode == MODE_EULER) {
+ joint->num = 3;
+ }
+ else {
+ if (num < 0) num = 0;
+ if (num > 3) num = 3;
+ joint->num = num;
+ }
+}
+
+
+void dJointSetAMotorAxis (int anum, int rel,
+ dReal x, dReal y, dReal z)
+{
+ assert(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ assert(!(!joint->node[1].body && (joint->flags & dJOINT_REVERSE) && rel == 1),"no first body, can't set axis rel=1");
+ assert(!(!joint->node[1].body && !(joint->flags & dJOINT_REVERSE) && rel == 2),"no second body, can't set axis rel=2");
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+
+ // adjust rel to match the internal body order
+ if (!joint->node[1].body && rel==2) rel = 1;
+
+ joint->rel[anum] = rel;
+
+ // 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;
+ r[0] = x;
+ r[1] = y;
+ r[2] = z;
+ r[3] = 0;
+ if (rel > 0) {
+ if (rel==1) {
+ dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r);
+ }
+ else {
+ // don't assert; handle the case of attachment to a bodiless geom
+ if (joint->node[1].body) { // jds
+ dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r);
+ }
+ else {
+ joint->axis[anum][0] = r[0]; joint->axis[anum][1] = r[1];
+ joint->axis[anum][2] = r[2]; joint->axis[anum][3] = r[3];
+ }
+ }
+ }
+ else {
+ joint->axis[anum][0] = r[0];
+ joint->axis[anum][1] = r[1];
+ joint->axis[anum][2] = r[2];
+ }
+ dNormalize3 (joint->axis[anum]);
+ if (joint->mode == MODE_EULER) amotorSetEulerReferenceVectors (joint);
+}
+
+
+void dJointSetAMotorAngle (int anum,
+ dReal angle)
+{
+ assert(joint && anum >= 0 && anum < 3);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ if (joint->mode == MODE_USER) {
+ if (anum < 0) anum = 0;
+ if (anum > 3) anum = 3;
+ joint->angle[anum] = angle;
+ }
+}
+
+
+void dJointSetAMotorParam (int parameter,
+ dReal value)
+{
+ assert(joint);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ int anum = parameter >> 8;
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ parameter &= 0xff;
+ joint->limot[anum].set (parameter, value);
+}
+
+
+void dJointSetAMotorMode (int mode)
+{
+ assert(joint);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ joint->mode = mode;
+ if (joint->mode == MODE_EULER) {
+ joint->num = 3;
+ amotorSetEulerReferenceVectors (joint);
+ }
+}
+
+
+int dJointGetAMotorNumAxes ()
+{
+ assert(joint);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ return joint->num;
+}
+
+
+void dJointGetAMotorAxis (int anum,
+ dVector3 result)
+{
+ assert(joint && anum >= 0 && anum < 3);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ if (joint->rel[anum] > 0) {
+ if (joint->rel[anum]==1) {
+ MathUtils.dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]);
+ }
+ else {
+ if (joint->node[1].body) { // jds
+ MathUtils.dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]);
+ }
+ else {
+ result[0] = joint->axis[anum][0]; result[1] = joint->axis[anum][1];
+ result[2] = joint->axis[anum][2]; result[3] = joint->axis[anum][3];
+ }
+ }
+ }
+ else {
+ result[0] = joint->axis[anum][0];
+ result[1] = joint->axis[anum][1];
+ result[2] = joint->axis[anum][2];
+ }
+}
+
+
+int dJointGetAMotorAxisRel (int anum)
+{
+ assert(joint && anum >= 0 && anum < 3);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ return joint->rel[anum];
+}
+
+
+dReal dJointGetAMotorAngle (int anum)
+{
+ assert(joint && anum >= 0 && anum < 3);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ if (anum < 0) anum = 0;
+ if (anum > 3) anum = 3;
+ return joint->angle[anum];
+}
+
+
+dReal dJointGetAMotorAngleRate (int anum)
+{
+ // @@@
+ dDebug (0,"not yet implemented");
+ return 0;
+}
+
+
+dReal dJointGetAMotorParam (int parameter)
+{
+ assert(joint);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ int anum = parameter >> 8;
+ if (anum < 0) anum = 0;
+ if (anum > 2) anum = 2;
+ parameter &= 0xff;
+ return joint->limot[anum].get (parameter);
+}
+
+
+int dJointGetAMotorMode ()
+{
+ assert(joint);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+ return joint->mode;
+}
+
+
+void dJointAddAMotorTorques (dReal torque1, dReal torque2, dReal torque3)
+{
+ dVector3 axes[3];
+ assert(joint);
+ assert(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+
+ if (joint->num == 0)
+ return;
+ assert((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints");
+
+ amotorComputeGlobalAxes (joint,axes);
+ axes[0][0] *= torque1;
+ axes[0][1] *= torque1;
+ axes[0][2] *= torque1;
+ if (joint->num >= 2) {
+ axes[0][0] += axes[1][0] * torque2;
+ axes[0][1] += axes[1][1] * torque2;
+ axes[0][2] += axes[1][2] * torque2;
+ if (joint->num >= 3) {
+ axes[0][0] += axes[2][0] * torque3;
+ axes[0][1] += axes[2][1] * torque3;
+ axes[0][2] += axes[2][2] * torque3;
+ }
+ }
+
+ if (joint->node[0].body != 0)
+ dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]);
+ if (joint->node[1].body != 0)
+ dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]);
+}
+
+
+dxJoint::Vtable __damotor_vtable = {
+ sizeof(dxJointAMotor),
+ (dxJoint::init_fn*) amotorInit,
+ (dxJoint::getInfo1_fn*) amotorGetInfo1,
+ (dxJoint::getInfo2_fn*) amotorGetInfo2,
+ dJointTypeAMotor};
+}
+*/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
