Kinematic PMoveable::odeToKinematic()
{
Kinematic k;
dQuaternion q_result, q_result1, q_base;
float norm;
const dReal *b_info;
const dReal *q_current = dBodyGetQuaternion(body);
q_base[0] = 0; q_base[1] = 0; q_base[2] = 0; q_base[3] = 1;
dQMultiply0(q_result1, q_current, q_base);
dQMultiply2(q_result, q_result1, q_current);
k.orientation_v = Vec3f(q_result[1], q_result[2], q_result[3]);
norm = sqrt(q_result[1] * q_result[1] + q_result[3] * q_result[3]);
if (norm == 0)
k.orientation = 0;
else
k.orientation = atan2(q_result[1] / norm, q_result[3] / norm);
b_info = dBodyGetPosition(body);
k.pos = Vec3f(b_info[0], b_info[1], b_info[2]);
b_info = dBodyGetLinearVel(body);
k.vel = Vec3f(b_info[0], b_info[1], b_info[2]);
return k;
}
dReal
dxJointUniversal::getAngle2()
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
dRtoQ( R, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross, qrel2 );
}
return - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
return 0;
}
void setFixedOrientation( dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row )
{
int s = info->rowskip;
int start_index = start_row * s;
// 3 rows to make body rotations equal
info->J1a[start_index] = 1;
info->J1a[start_index + s + 1] = 1;
info->J1a[start_index + s*2+2] = 1;
if ( joint->node[1].body )
{
info->J2a[start_index] = -1;
info->J2a[start_index + s+1] = -1;
info->J2a[start_index + s*2+2] = -1;
}
// compute the right hand side. the first three elements will result in
// relative angular velocity of the two bodies - this is set to bring them
// back into alignment. the correcting angular velocity is
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * u
// = (erp*fps) * theta * u
// where rotation along unit length axis u by theta brings body 2's frame
// to qrel with respect to body 1's frame. using a small angle approximation
// for sin(), this gives
// angular_velocity = (erp*fps) * 2 * v
// where the quaternion of the relative rotation between the two bodies is
// q = [cos(theta/2) sin(theta/2)*u] = [s v]
// get qerr = relative rotation (rotation error) between two bodies
dQuaternion qerr, e;
if ( joint->node[1].body )
{
dQuaternion qq;
dQMultiply1( qq, joint->node[0].body->q, joint->node[1].body->q );
dQMultiply2( qerr, qq, qrel );
}
else
{
dQMultiply3( qerr, joint->node[0].body->q, qrel );
}
if ( qerr[0] < 0 )
{
qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
qerr[2] = -qerr[2];
qerr[3] = -qerr[3];
}
dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
dReal k = info->fps * info->erp;
info->c[start_row] = 2 * k * e[0];
info->c[start_row+1] = 2 * k * e[1];
info->c[start_row+2] = 2 * k * e[2];
}
dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis,
dQuaternion q_initial )
{
// get qrel = relative rotation between the two bodies
dQuaternion qrel;
if ( body2 )
{
dQuaternion qq;
dQMultiply1( qq, body1->q, body2->q );
dQMultiply2( qrel, qq, q_initial );
}
else
{
// pretend body2->q is the identity
dQMultiply3( qrel, body1->q, q_initial );
}
return getHingeAngleFromRelativeQuat( qrel, axis );
}
void testQuaternionMultiply()
{
HEADER;
dMatrix3 RA,RB,RC,Rtest;
dQuaternion qa,qb,qc;
dReal diff,maxdiff=0;
for (int i=0; i<100; i++) {
makeRandomRotation (RB);
makeRandomRotation (RC);
dRtoQ (RB,qb);
dRtoQ (RC,qc);
dMultiply0 (RA,RB,RC,3,3,3);
dQMultiply0 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply1 (RA,RB,RC,3,3,3);
dQMultiply1 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply2 (RA,RB,RC,3,3,3);
dQMultiply2 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply0 (RA,RC,RB,3,3,3);
transpose3x3 (RA);
dQMultiply3 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
void
dxJointUniversal::getAngles( dReal *angle1, dReal *angle2 )
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
dRtoQ( R, qcross );
// This code is essentialy the same as getHingeAngle(), see the comments
// there for details.
// get qrel = relative rotation between node[0] and the cross
dQMultiply1( qq, node[0].body->q, qcross );
dQMultiply2( qrel, qq, qrel1 );
*angle1 = getHingeAngleFromRelativeQuat( qrel, axis1 );
// This is equivalent to
// dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
// You see that the R is constructed from the same 2 axis as for angle1
// but the first and second axis are swapped.
// So we can take the first R and rapply a rotation to it.
// The rotation is around the axis between the 2 axes (ax1 and ax2).
// We do a rotation of 180deg.
dQuaternion qcross2;
// Find the vector between ax1 and ax2 (i.e. in the middle)
// We need to turn around this vector by 180deg
// The 2 axes should be normalize so to find the vector between the 2.
// Add and devide by 2 then normalize or simply normalize
// ax2
// ^
// |
// |
/// *------------> ax1
// We want the vector a 45deg
//
// N.B. We don't need to normalize the ax1 and ax2 since there are
// normalized when we set them.
// We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z]
qrel[0] = 0; // equivalent to cos(Pi/2)
qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1
qrel[2] = ax1[1] + ax2[1];
qrel[3] = ax1[2] + ax2[2];
dReal l = dRecip( sqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] ) );
qrel[1] *= l;
qrel[2] *= l;
qrel[3] *= l;
dQMultiply0( qcross2, qrel, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross2 );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross2, qrel2 );
}
*angle2 = - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
else
{
*angle1 = 0;
*angle2 = 0;
}
}
void SCylinderParts::set(dWorldID w, dSpaceID space)
{
double radius = m_cmpnt.radius();
double length = m_cmpnt.length();
// konao
//LOG_MSG(("[SCylinderParts::set] ODE geom created (r, l)=(%f, %f) [%s:%d]\n", radius, length, __FILE__, __LINE__))
// TODO: Ideally, cylinder should be constructed here. However, collision detection
// between two cylinders could not be realized. So, Capsule is required
// by okamoto@tome on 2011-10-12
//dGeomID geom = dCreateCapsule(0, radius, length);
dGeomID geom = dCreateCylinder(0, radius, length);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
//dMassSetCapsule(&m, DENSITY, 1, radius, length);
dMassSetCylinder(&m, DENSITY, 1, radius, length); //TODO: mass of the cylinder should be configurable
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body
// set the long axis as y axis
dReal offq[4] = {0.707, 0.707, 0.0, 0.0};
dReal offq2[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()};
dQuaternion qua;
dQMultiply2(qua, offq2, offq);
dGeomSetOffsetQuaternion(geom, qua);
//dGeomSetOffsetQuaternion(geom, offq2);
//dReal tmpq[4] = {0.707, 0.0, 0.0, 0.707};
//dGeomSetQuaternion(geom, tmpq);
//dBodySetQuaternion(body, tmpq);
/*TODO: Consideration is required whether this procedure is needed
* Reflection of orientation of the cylinder
* dMatrix3 R;
* dRFromAxisAndAngle(dMatrix3 R, dReal rx, dReal ry, dReal rz, dReal angle)
* dRFromAxisAndAngle(R,x_axis,y_axis,z_axis,angleData);
* dBodySetRotation(body,R); // Request of actual rotation
*/
// Not used, deleted by inamura
// real part of the quaternion
// double q = cos(angleData/2.0);
// imaginary part of the quaternion
// double i,j,k;
// i = x_axis * sin(angleData/2.0);
// j = y_axis * sin(angleData/2.0);
// k = z_axis * sin(angleData/2.0);
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
void SParts::calcPosition(Joint *currj, Joint *nextj, const Vector3d &anchorv, const Rotation &R)
{
if (currj) {
DUMP(("currj = %s\n", currj->name()));
}
DUMP(("name = %s\n", name()));
if (nextj) {
DUMP(("nextj = %s\n", nextj->name()));
}
DUMP(("anchorv = (%f, %f, %f)\n", anchorv.x(), anchorv.y(), anchorv.z()));
{
Vector3d v(m_pos.x(), m_pos.y(), m_pos.z());
// Calculate shift vector from positoin/anchor/orientation
if (currj) {
v -= currj->getAnchor();
DUMP(("parent anchor = %s (%f, %f, %f)\n",
currj->name(),
currj->getAnchor().x(),
currj->getAnchor().y(), currj->getAnchor().z()));
}
DUMP(("v1 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
Rotation rr(R);
if (currj) {
rr *= currj->getRotation();
}
v.rotate(rr);
DUMP(("v2 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
v += anchorv;
DUMP(("v3 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
//DUMP(("v4 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
setPosition(v);
if(m_onGrasp) {
dBodyID body = odeobj().body();
//int num = dBodyGetNumJoints(body);
// get grasp joint
dJointID joint = dBodyGetJoint(body, 0);
// get target body from joint
dBodyID targetBody = dJointGetBody(joint, 1);
// get position of grasp target and parts
const dReal *pos = dBodyGetPosition(body);
const dReal *tpos = dBodyGetPosition(targetBody);
// Set if it is moved
if(pos[0] != tpos[0] || pos[1] != tpos[1] || pos[2] != tpos[2]) {
dBodySetPosition(targetBody, pos[0], pos[1], pos[2]);
}
// get position of grasp target and parts
const dReal *qua = dBodyGetQuaternion(body);
const dReal *tqua = dBodyGetQuaternion(targetBody);
// rotation from initial orientation
dQuaternion rot;
dQMultiply2(rot, qua, m_gini);
// Set if it is rotated
if(rot[0] != tqua[0] || rot[1] != tqua[1] || rot[2] != tqua[2] || rot[3] != tqua[3]) {
dBodySetQuaternion(targetBody, rot);
}
//LOG_MSG(("target2 %d", targetBody));
//LOG_MSG(("(%f, %f, %f)", tpos[0], tpos[1], tpos[2]));
//LOG_MSG(("onGrasp!! num = %d, joint = %d", num));
}
}
Rotation rr(R);
rr *= m_rot;
if (currj) {
rr *= currj->getRotation();
}
setRotation(rr);
if (!nextj) { return; }
for (ChildC::iterator i=m_children.begin(); i!=m_children.end(); i++) {
Child *child = *i;
Rotation R_(R);
if (currj) {
R_ *= currj->getRotation();
}
Vector3d nextv;
nextv = nextj->getAnchor();
if (currj) {
nextv -= currj->getAnchor();
}
nextv.rotate(R_);
nextv += anchorv;
DUMP(("nextv1 : (%f, %f, %f)\n", nextv.x(), nextv.y(), nextv.z()));
// act in a case that multiple JOINTs are connected to one link
if(strcmp(nextj->name(),child->currj->name()) == 0) {
child->nextp->calcPosition(child->currj, child->nextj, nextv, R_);
}
}
}