[ODE] dJointGetHinge2Angle2

[LR]

Great idea, i've made the folowing modifications:


1] In joint.cpp,

** ADDED:
dReal dJointGetHinge2Angle2 (dJointID j)
{
  dxJointHinge2* joint = (dxJointHinge2*)j;
  dAASSERT(joint);
  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
  if (joint->node[0].body) {
    dReal ang = getHingeAngle
(joint->node[0].body,joint->node[1].body,joint->axis1,
			  joint->qrel);
	if (joint->flags & dJOINT_REVERSE)
	   return -ang;
	else
	   return ang;
  }
  else return 0;
} 

** ADDED
static void hinge2ComputeInitialRelativeRotation (dxJointHinge2 *joint)
{
  if (joint->node[0].body) {
    if (joint->node[1].body) {
      dQMultiply1
(joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
    }
    else {
      // set joint->qrel to the transpose of the first body q
      joint->qrel[0] = joint->node[0].body->q[0];
      for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
    }
  }
}

** IN static void makeHinge2V1andV2 (dxJointHinge2 *joint),
Added hinge2ComputeInitialRelativeRotation(joint); at the end of the IF()
code 


2] In ojects.h

** ADDED
ODE_API dReal dJointGetHinge2Angle2 (dJointID);


3] In Joint.h
** ADDED dQuaternion qrel;	at the end of struct dxJointHinge2 : public
dxJoint structure

It seems to work fine in my simulation but ... It may be checked by someone
who know how to add to vectors :))

LR

-----Message d'origine-----
De : ode-bounces at ode.org [mailto:ode-bounces at ode.org] De la part de Remi
Ricard
Envoyé : jeudi 3 mai 2007 16:47
À : ODE Mailing List (E-mail)
Objet : Re: [ODE] dJointGetHinge2Angle2

Hi,

 > In the hinge2 code we can see:
 >
 > static dReal measureHinge2Angle (dxJointHinge2 *joint)  > {
 >   dVector3 a1,a2;
 >   dMULTIPLY0_331 (a1,joint->node[1].body->posr.R,joint->axis2);
 >   dMULTIPLY1_331 (a2,joint->node[0].body->posr.R,a1);
 >   dReal x = dDOT(joint->v1,a2);
 >   dReal y = dDOT(joint->v2,a2);
 >   return -dAtan2 (y,x);
 > }
 >
 > This code is called to know the vertical axis angle. Does someone know
how to get the angle of the horizontal axis ?
 >
 > I saw in the archive mails that it is a recurent question ... then if
someone know how to do it will help a lot of ode users :)


I took a look in the code and there is no information of the relative
orientation of Body1 and Body2 along the horizontal axis.

Other joint have a Quaternion (qrel) to keep the initial difference in
orientation but the Hinge2 uses 2 vectors to help retrieve the vertical
angle.

If you replace those 2 vectors by a qrel you should be able to find both
angles.

You can look at the Universal joint to implement the code.

Remi
_______________________________________________
ODE mailing list
ODE at ode.org
http://ode.org/mailman/listinfo/ode