146 lines
4.9 KiB
C++
146 lines
4.9 KiB
C++
#ifdef _WIN32
|
|
#include <windows.h>
|
|
#include <GL/gl.h>
|
|
#include <GL/glu.h>
|
|
#else
|
|
#ifdef __APPLE__
|
|
#include <OpenGL/gl.h>
|
|
#include <OpenGL/glu.h>
|
|
#else
|
|
#include <GL/gl.h>
|
|
#include <GL/glu.h>
|
|
#endif
|
|
#endif
|
|
|
|
#include "myworld.h"
|
|
|
|
using namespace std;
|
|
|
|
void glsubBox(double dx, double dy, double dz)
|
|
{
|
|
glBegin(GL_QUADS);
|
|
glNormal3d(1, 0, 0); glVertex3d(dx/2, -dy/2, dz/2); glVertex3d(dx/2, -dy/2, -dz/2); glVertex3d(dx/2, dy/2, -dz/2); glVertex3d(dx/2, dy/2, dz/2);
|
|
glEnd();
|
|
glBegin(GL_QUADS);
|
|
glNormal3d(0, 1, 0); glVertex3d(dx/2, dy/2, dz/2); glVertex3d(dx/2, dy/2, -dz/2); glVertex3d(-dx/2, dy/2, -dz/2); glVertex3d(-dx/2, dy/2, dz/2);
|
|
glEnd();
|
|
glBegin(GL_QUADS);
|
|
glNormal3d(-1, 0, 0); glVertex3d(-dx/2, dy/2, -dz/2); glVertex3d(-dx/2, -dy/2, -dz/2); glVertex3d(-dx/2, -dy/2, dz/2); glVertex3d(-dx/2, dy/2, dz/2);
|
|
glEnd();
|
|
glBegin(GL_QUADS);
|
|
glNormal3d(0, -1, 0); glVertex3d(-dx/2, -dy/2, dz/2); glVertex3d(-dx/2, -dy/2, -dz/2); glVertex3d(dx/2, -dy/2, -dz/2); glVertex3d(dx/2, -dy/2, dz/2);
|
|
glEnd();
|
|
glBegin(GL_QUADS);
|
|
glNormal3d(0, 0, 1); glVertex3d(dx/2, -dy/2, dz/2); glVertex3d(dx/2, dy/2, dz/2); glVertex3d(-dx/2, dy/2, dz/2); glVertex3d(-dx/2, -dy/2, dz/2);
|
|
glEnd();
|
|
glBegin(GL_QUADS);
|
|
glNormal3d(0, 0, -1); glVertex3d(dx/2, -dy/2, -dz/2); glVertex3d(-dx/2, -dy/2, -dz/2); glVertex3d(-dx/2, dy/2, -dz/2); glVertex3d(dx/2, dy/2, -dz/2);
|
|
glEnd();
|
|
}
|
|
|
|
//=============================================================
|
|
// RigidBody
|
|
//=============================================================
|
|
void RigidBody::render()
|
|
{
|
|
glColor3d(_color[0],_color[1],_color[2]);
|
|
glPushMatrix();
|
|
glMultMatrixd(getPoseGlobal().GetArray()); // move to {body}
|
|
glMultMatrixd(_T0.GetArray()); // move to box's reference frame
|
|
glsubBox(_Lx, _Ly, _Lz);
|
|
glPopMatrix();
|
|
}
|
|
|
|
//=============================================================
|
|
// MyWorld
|
|
//=============================================================
|
|
bool MyWorld::create()
|
|
{
|
|
|
|
double Lx = 0.3, Ly = 0.05, Lz = 0.05; // link dimension
|
|
double M = 1; // link mass
|
|
double Ixx = 1./12.*M*(Ly*Ly+Lz*Lz), Iyy = 1./12.*M*(Lx*Lx+Lz*Lz), Izz = 1./12.*M*(Lx*Lx+Ly*Ly), Ixy = 0, Ixz = 0, Iyz = 0; // moments of inertia with respect to the mass center
|
|
SE3 T_link_com = SE3(); // transformation from {link} to {center of mass} (In this example, {link} == {center of mass}, so T_link_com = identity.)
|
|
|
|
// set mass properties of the links
|
|
link1.setMass(M, Ixx, Iyy, Izz, Ixy, Ixz, Iyz, T_link_com); // set mass and the moments of inertia of the link (with respect to {link1})
|
|
link2.setMass(M, Ixx, Iyy, Izz, Ixy, Ixz, Iyz, T_link_com);
|
|
link3.setMass(M, Ixx, Iyy, Izz, Ixy, Ixz, Iyz, T_link_com);
|
|
link4.setMass(M, Ixx, Iyy, Izz, Ixy, Ixz, Iyz, T_link_com);
|
|
|
|
// set box rendering (optional)
|
|
link1.setBoxRendering(0.4*Lx, Ly, Lz, SE3(), Vec3(0,0,1));
|
|
link2.setBoxRendering(Lx, Ly, Lz, SE3(), Vec3(1,0,0));
|
|
link3.setBoxRendering(Lx, Ly, Lz, SE3(), Vec3(0,1,0));
|
|
link4.setBoxRendering(Lx, Ly, Lz, SE3(), Vec3(1,1,0));
|
|
|
|
// set link name
|
|
link1.setName("link1");
|
|
link2.setName("link2");
|
|
link3.setName("link3");
|
|
link4.setName("link4");
|
|
|
|
// connect the links with joints
|
|
joint1.connectBodies(&ground, &link1); joint1.setPosition(Vec3(0,0,0), Vec3(-0.2*Lx,0,0));
|
|
joint2.connectBodies(&link1, &link2); joint2.setPosition(Vec3(0.2*Lx,0,0), Vec3(-0.5*Lx,0,0));
|
|
joint3.connectBodies(&link2, &link3); joint3.setPosition(Vec3(0.5*Lx,0,0), Vec3(-0.5*Lx,0,0));
|
|
joint4.connectBodies(&link3, &link4); joint4.setPosition(Vec3(0.5*Lx,0,0), Vec3(-0.5*Lx,0,0));
|
|
joint5.connectBodies(&link4, &ground); joint5.setPosition(Vec3(0,0,0), Vec3(2*Lx,0,0.5*Lx)); //joint5.setOrientation(Exp(0,0,0),Exp(0,45*3.14159/180.,0));
|
|
|
|
// set rotating axes of the revolute joints
|
|
joint1.setAxis(0,1,0);
|
|
joint2.setAxis(0,1,0);
|
|
joint3.setAxis(0,1,0);
|
|
joint4.setAxis(0,1,0);
|
|
joint5.setAxis(1,0,0);
|
|
|
|
// set prescribed/unprescribed joints
|
|
joint1.setPrescribed(true);
|
|
joint2.setPrescribed(false);
|
|
joint3.setPrescribed(false);
|
|
joint4.setPrescribed(false);
|
|
joint5.setPrescribed(false);
|
|
|
|
// set joint name
|
|
joint1.setName("joint1");
|
|
joint2.setName("joint2");
|
|
joint3.setName("joint3");
|
|
joint4.setName("joint4");
|
|
joint5.setName("joint5");
|
|
|
|
// build system
|
|
if ( !system.buildSystem(&ground) ) return false;
|
|
if ( !system.enforceConstraints() ) {
|
|
cout << "failed in enforcing constraints!" << endl;
|
|
}
|
|
system.updateKinematics();
|
|
|
|
// set gravity
|
|
system.setGravity(Vec3(0,0,-9.81));
|
|
|
|
// init
|
|
joint1.set_q(0.0);
|
|
joint1.set_dq(2.0);
|
|
joint1.set_ddq(0.0);
|
|
system.enforceConstraints();
|
|
system.updateKinematics();
|
|
|
|
//ofstream fout("dd.txt");
|
|
//fout << system.getInfoStr() << endl;
|
|
//fout.close();
|
|
|
|
return true;
|
|
}
|
|
|
|
void MyWorld::render()
|
|
{
|
|
for (std::list<GBody*>::iterator iter_pbody = system.pBodies.begin(); iter_pbody != system.pBodies.end(); iter_pbody++) {
|
|
(*iter_pbody)->render();
|
|
}
|
|
}
|
|
|
|
void MyWorld::testfunc()
|
|
{
|
|
}
|
|
|