[ODE] An embarassingly basic geometry question

[Shamyl Zakariya]

All
I'm working on modeling an air piston, and as such I'd like to draw the 
cylinder connecting two ball joints. For the time being, I'm just 
trying to draw a cylinder between two points; when I get the rotation 
code working right, I'll use it to position cylindrical bodies to 
represent the tube and piston, but not until I get the orientation 
correct. So this is just temporary prototyping.

Now, the approach I'm taking to draw a cylinder between point a & b, in 
pseudo code, is:
	-create a point directly over b, named c, to make triangle a,b,c; c[0] 
= b[0], c[1] = b[1] & c[2] = b[2] + piston length
	-calculate normal to this triangle, using vectors ba and bc, this is 
the axis the cylinder will be rotated on
	-calculate angle between the vertical line (bc) and the line the 
piston should be on (ba) via arc cosine of the dot product of ba and bc
	-draw a cylinder with length matching the distance between a&b, 
rotated by the angle calculated above, on the axis calculated above.

I assumed this would work as long as the right/left hand ruling were 
consistent, but in reality, it doesn't work well at all. The cylinder 
initially is positioned correctly, but goes askew when the orientation 
of the bodies changes. Hard to describe. If you want, I'll send a 
screenshot. It seems it goes askew when the endpoints of the cylinder 
are not perfectly flat or vertical.

I'm embarrassed, but really, this kind of geometry isn't my strong 
point.

Here's my code:

void Piston::draw( void )
{
	... //extra junk cut out

	//the pinions are the attachment points (ball joints) for the piston
	const dReal *anchorA = dBodyGetPosition( _bodyAPinion );
	const dReal *anchorB = dBodyGetPosition( _bodyBPinion );
	dReal pistonLength = distance( (dReal*) anchorA, (dReal*) anchorB);

	//make arbitrary point over anchorB
	dReal anchorC[3];
	anchorC[0] = anchorB[0];
	anchorC[1] = anchorB[1];
	anchorC[2] = anchorB[2] + pistonLength;
	
	//calculate axis and angle
	dVector3 axis;
	normal( (dReal *) anchorA, (dReal *) anchorB, (dReal *) anchorC, axis 
);	
	dReal d = dot( (dReal *) anchorA, (dReal *) anchorB, (dReal *) anchorC 
);
	dReal angle = acos (d);
		
	printf("axis: (%.2f, %.2f, %.2f); dot: %.2f, angle: %.2f\n", axis[0], 
axis[1], axis[2], d, angle );

	//now create piston geometries
	dVector3 midpoint;
	midpoint[0] = (anchorA[0] + anchorB[0]) / 2.0;
	midpoint[1] = (anchorA[1] + anchorB[1]) / 2.0;
	midpoint[2] = (anchorA[2] + anchorB[2]) / 2.0;
	
	dMatrix3 R;
	dRFromAxisAndAngle( R, axis[0], axis[1], axis[2], angle );
	
	dsSetColor( 0.4, 0.4, 0.5 );
	dsDrawCylinder( midpoint, R, pistonLength, 0.1 );
}

And here are the dot, cross product and normal and distance functions:

void cross(dVector3 p1, dVector3 p2, dVector3 result) {
	result[0] =(p1[1] * p2[2] ) - (p1[2] * p2[1] );
	result[1] =(p1[2] * p2[0] ) - (p1[0] * p2[2] );
	result[2] =(p1[0] * p2[1] ) - (p1[1] * p2[0] );
}

dReal dot(dVector3 v1, dVector3 v2) {
	return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] );
}

dReal distance(dReal *p1, dReal *p2) {
	dReal dx = p1[0] - p2[0], dy = p1[1] - p2[1], dz = p1[2] - p2[2];
	return (sqrt((dx*dx)+(dy*dy)+(dz*dz)));
}

/*
	returns the normal to the plane defined by triangle abc as
	b->a & b->c
*/
void normal( dReal *a, dReal *b, dReal *c, dVector3 normal )
{
	dVector3 ba, bc;
	ba[0] = b[0] - a[0];
	ba[1] = b[1] - a[1];
	ba[2] = b[2] - a[2];

	bc[0] = b[0] - c[0];
	bc[1] = b[1] - c[1];
	bc[2] = b[2] - c[2];
	
	cross( ba, bc, normal );
}

void normalize( dVector3 v )
{
	dReal mag = sqrt( (v[0]*v[0]) + (v[1]*v[1]) + (v[2]*v[2]) );
	if (mag > 0)
	{
		v[0] /= mag;
		v[1] /= mag;
		v[2] /= mag;
	}
}

/*
	returns the dot between three points,
	a->b & b->c
*/
dReal dot( dReal *a, dReal *b, dReal *c )
{
	dVector3 ba, bc;
	ba[0] = b[0] - a[0];
	ba[1] = b[1] - a[1];
	ba[2] = b[2] - a[2];
	normalize(ba);

	bc[0] = b[0] - c[0];
	bc[1] = b[1] - c[1];
	bc[2] = b[2] - c[2];
	normalize(bc);
	
	return dot( ba, bc );
}
	

For what it's worth, the piston starts misaligning when the calculated 
angle goes past PI/2.


shamyl zakariya : atomosuprabuddhavista