void
dxJointBall::setRelativeValues()
{
dVector3 anchor;
dJointGetBallAnchor(this, anchor);
setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
}
void CBallJoint::RenderDebug()
{
if (IsAttached())
{
matrix44 Tfm;
dVector3 CurrAnchor;
dJointGetBallAnchor(ODEJointID, CurrAnchor);
Tfm.scale(vector3(0.1f, 0.1f, 0.1f));
Tfm.translate(vector3(CurrAnchor[0], CurrAnchor[1], CurrAnchor[2]));
nGfxServer2::Instance()->DrawShape(nGfxServer2::Sphere, Tfm, GetDebugVisualizationColor());
}
}
/* returns the joint anchoring point */
void joint_anchor (t_real *anchor, dJointID j) {
dVector3 joint_position;
int joint_type;
joint_type = dJointGetType (j);
if (joint_type == dJointTypeBall) {
dJointGetBallAnchor (j, joint_position);
}
else {
err_message ("Unrecognized joint type\n");
exit (EXIT_FAILURE);
}
vector_set (anchor, joint_position [0], joint_position [1], joint_position [2]);
}
static void get_phys_joint_anchor(dJointID j, float *v)
{
dVector3 V = { 0, 0, 0 };
switch (dJointGetType(j))
{
case dJointTypeBall: dJointGetBallAnchor (j, V); break;
case dJointTypeHinge: dJointGetHingeAnchor (j, V); break;
case dJointTypeHinge2: dJointGetHinge2Anchor (j, V); break;
case dJointTypeUniversal: dJointGetUniversalAnchor(j, V); break;
default: break;
}
v[0] = (float) V[0];
v[1] = (float) V[1];
v[2] = (float) V[2];
}
int main( void ) {
dWorldID world = dWorldCreate();
dJointID joint = dJointCreateBall( world, 0 );
dVector3 pos;
printf( "Create world and joint. Setting joint anchor to [4,11.18,-1.2]...\n" );
dJointSetBallAnchor( joint, 4.0, 11.18, -1.2 );
printf( "Done. Fetching set values...\n" );
dJointGetBallAnchor( joint, pos );
printf( "Anchor is at: [%0.5f, %0.5f, %0.5f]", pos[0], pos[1], pos[2] );
dJointDestroy( joint );
dWorldDestroy( world );
return 0;
}
void BallJoint::drawObject(const VisualizationParameterSet& visParams)
{
if(visParams.surfaceStyle == VisualizationParameterSet::PHYSICS_WIREFRAME ||
visParams.surfaceStyle == VisualizationParameterSet::PHYSICS_FLAT ||
visParams.surfaceStyle == VisualizationParameterSet::PHYSICS_SMOOTH ||
visParams.surfaceStyle == VisualizationParameterSet::MIXED_APPEARANCE ||
visParams.surfaceStyle == VisualizationParameterSet::MIXED_PHYSICS)
{
dVector3 tempA;
dJointGetBallAnchor(this->physicalJoint, tempA);
Vector3d tempAnchor(tempA[0],tempA[1], tempA[2]);
if(visParams.surfaceStyle == VisualizationParameterSet::MIXED_APPEARANCE)
{
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
glShadeModel(GL_FLAT);
GLHelper::getGLH()->drawBallJoint(tempAnchor, true);
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
glShadeModel(GL_SMOOTH);
}
else
GLHelper::getGLH()->drawBallJoint(tempAnchor, false);
}
}
//! Refer the current anchor position in world coordinate
// added by noma on 2012-02-27
Vector3d BallJoint::getCurrentAnchorPosition()
{
dReal tmpPos[4];
dJointGetBallAnchor(m_joint, tmpPos);
return Vector3d(tmpPos[0],tmpPos[1],tmpPos[2]);
}
void odCable::exportVTK(FILE *stream){
int i;
double segLen=length/(double)nSegments;
dVector3 P, j0, j1;
odPoint d;
fprintf(stream,"<?xml version=\"1.0\"?>\n");
fprintf(stream,"<VTKFile type=\"PolyData\" version=\"0.1\" byte_order=\"LittleEndian\">\n");
fprintf(stream,"<PolyData>\n");
fprintf(stream," <Piece NumberOfPoints=\"%i\" NumberOfLines=\"%i\" >\n", nSegments*2, nSegments);
fprintf(stream," <Points>\n");
fprintf(stream," <DataArray type=\"Float32\" NumberOfComponents=\"3\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
dBodyGetRelPointPos (element[i], -segLen/2., 0, 0, P);
fprintf(stream,"%lf %lf %lf ", P[0], P[1], P[2]);
dBodyGetRelPointPos (element[i], segLen/2., 0, 0, P);
fprintf(stream,"%lf %lf %lf\n", P[0], P[1], P[2]);
}
fprintf(stream," </DataArray>\n");
fprintf(stream," </Points>\n");
fprintf(stream," <Lines>\n");
fprintf(stream," <DataArray type=\"Int32\" Name=\"connectivity\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
fprintf(stream,"%i %i\n",2*i, 2*i+1);
}
fprintf(stream," </DataArray>\n");
fprintf(stream," <DataArray type=\"Int32\" Name=\"offsets\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
fprintf(stream,"%i\n", 2*i+2);
}
fprintf(stream," </DataArray>\n");
fprintf(stream," </Lines>\n");
/*
fprintf(stream," <PointData>\n");
fprintf(stream," <DataArray type=\"Float32\" Name=\"Force\" NumberOfComponents=\"3\" format=\"ascii\">\n");
fprintf(stream,"%lf %lf %lf ", force1.x, force1.y, force1.z);
fprintf(stream,"%lf %lf %lf\n", force2.x, force2.y, force2.z);
fprintf(stream," </DataArray>\n");
fprintf(stream," </PointData>\n");
*/
fprintf(stream," <CellData>\n");
fprintf(stream," <DataArray type=\"Float32\" Name=\"Element Length\" NumberOfComponents=\"1\" format=\"ascii\">\n");
// First length from end1 to joint 0
dJointGetBallAnchor(end1, j0);
dJointGetBallAnchor(joint[0], j1);
d=odPoint(j0[0], j0[1], j0[2])-odPoint(j1[0], j1[1], j1[2]);
fprintf(stream,"%lf\n", d.mag());
for (i=0;i<nSegments-2;i++){
dJointGetBallAnchor(joint[i], j0);
dJointGetBallAnchor(joint[i+1], j1);
d=odPoint(j0[0], j0[1], j0[2])-odPoint(j1[0], j1[1], j1[2]);
fprintf(stream,"%lf\n", d.mag());
}
fprintf(stream,"%lf\n", 0);
fprintf(stream," </DataArray>\n");
fprintf(stream," </CellData>\n");
fprintf(stream," <CellData>\n");
fprintf(stream," <DataArray type=\"Int32\" Name=\"Element ID\" NumberOfComponents=\"1\" format=\"ascii\">\n");
for (i=0;i<nSegments;i++){
fprintf(stream,"%i\n", i);
}
fprintf(stream,"%lf\n", 0);
fprintf(stream," </DataArray>\n");
fprintf(stream," </CellData>\n");
fprintf(stream," </Piece>\n");
fprintf(stream,"</PolyData>\n");
fprintf(stream,"</VTKFile>\n");
}
