void drawgeom(const GeometricPrimitive3D& geom)
{
switch(geom.type) {
case GeometricPrimitive3D::Point:
{
glBegin(GL_POINTS);
glVertex3v(*AnyCast<Vector3>(&geom.data));
glEnd();
}
break;
case GeometricPrimitive3D::Segment:
{
const Segment3D* seg=AnyCast<Segment3D>(&geom.data);
glBegin(GL_LINES);
glVertex3v(seg->a);
glVertex3v(seg->b);
glEnd();
}
break;
/*
case GeometricPrimitive3D::Circle:
{
const Circle3D* circle = AnyCast<Circle3D>(&geom.data);
glPushMatrix();
glTranslate(circle->center);
drawCircle(circle->axis,circle->radius);
glPopMatrix();
}
break;
*/
/*
case GeometricPrimitive3D::AABB:
{
const AABB3D* aabb=AnyCast<AABB3D>(&geom.data);
drawBoundingBox(aabb->bmin,aabb->bmax);
}
break;
*/
case GeometricPrimitive3D::Box:
{
const Box3D* box=AnyCast<Box3D>(&geom.data);
Matrix4 m;
box->getBasis(m);
glPushMatrix();
glMultMatrix(m);
drawBoxCorner(box->dims.x,box->dims.y,box->dims.z);
glPopMatrix();
break;
}
case GeometricPrimitive3D::Triangle:
{
const Triangle3D* tri=AnyCast<Triangle3D>(&geom.data);
drawTriangle(tri->a,tri->b,tri->c);
break;
}
case GeometricPrimitive3D::Polygon:
{
const Polygon3D* p=AnyCast<Polygon3D>(&geom.data);
Plane3D plane;
p->getPlane(0,plane);
glNormal3v(plane.normal);
glBegin(GL_TRIANGLE_FAN);
glVertex3v(p->vertices[0]);
for(size_t i=1;i+1<p->vertices.size();i++) {
glVertex3v(p->vertices[i]);
glVertex3v(p->vertices[i+1]);
}
glEnd();
break;
}
case GeometricPrimitive3D::Sphere:
{
const Sphere3D* s=AnyCast<Sphere3D>(&geom.data);
glPushMatrix();
glTranslate(s->center);
drawSphere(s->radius,32,32);
glPopMatrix();
break;
}
case GeometricPrimitive3D::Cylinder:
{
const Cylinder3D* s=AnyCast<Cylinder3D>(&geom.data);
glPushMatrix();
glTranslate(s->center);
drawCylinder(s->axis*s->height,s->radius,32);
glPopMatrix();
break;
}
break;
default:
fprintf(stderr,"draw: Unsupported geometry type\n");
return;
}
}
static void GLDrawLine(const Vector3r& from, const Vector3r& to, const Vector3r& color=Vector3r(1,1,1)){
glEnable(GL_LIGHTING); glColor3v(color);
glBegin(GL_LINES); glVertex3v(from); glVertex3v(to); glEnd();
}
void
cb_display()
{
BeginDraw();
int i;
PQP_CollideResult cres;
PQP_DistanceResult dres;
PQP_ToleranceResult tres;
double oglm[16];
switch(query_type)
{
case 0:
// draw model 1
glColor3f(1,1,1); // setup color and transform
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus1_drawn->Draw(); // do gl rendering
glPopMatrix(); // restore transform
// draw model 2
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus2_drawn->Draw();
glPopMatrix();
break;
case 1:
// perform collision query
PQP_Collide(&cres,R1[step],T1[step],torus1_tested,
R2[step],T2[step],torus2_tested,
PQP_ALL_CONTACTS);
// draw model 1 and its overlapping tris
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
glColor3f(1,1,1);
torus1_drawn->Draw();
glColor3f(1,0,0);
for(i = 0; i < cres.NumPairs(); i++)
{
torus1_drawn->DrawTri(cres.Id1(i));
}
glPopMatrix();
// draw model 2 and its overlapping tris
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
glColor3f(1,1,1);
torus2_drawn->Draw();
glColor3f(1,0,0);
for(i = 0; i < cres.NumPairs(); i++)
{
torus2_drawn->DrawTri(cres.Id2(i));
}
glPopMatrix();
break;
case 2:
// perform distance query
PQP_Distance(&dres,R1[step],T1[step],torus1_tested,
R2[step],T2[step],torus2_tested,
0.0,0.0);
// draw models
glColor3f(1,1,1);
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus1_drawn->Draw();
glPopMatrix();
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus2_drawn->Draw();
glPopMatrix();
// draw the closest points as small spheres
glColor3f(0,1,0);
PQP_REAL P1[3],P2[3],V1[3],V2[3];
VcV(P1,dres.P1());
VcV(P2,dres.P2());
// each point is in the space of its model;
// transform to world space
MxVpV(V1,R1[step],P1,T1[step]);
glPushMatrix();
glTranslated(V1[0],V1[1],V1[2]);
glutSolidSphere(.01,15,15);
glPopMatrix();
MxVpV(V2,R2[step],P2,T2[step]);
glPushMatrix();
glTranslated(V2[0],V2[1],V2[2]);
glutSolidSphere(.01,15,15);
glPopMatrix();
// draw the line between the closest points
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3v(V1);
glVertex3v(V2);
glEnd();
glEnable(GL_LIGHTING);
break;
case 3:
// perform tolerance query
PQP_Tolerance(&tres,R1[step],T1[step],torus1_tested,
R2[step],T2[step],torus2_tested,
tolerance);
if (tres.CloserThanTolerance())
glColor3f(0,0,1);
else
glColor3f(1,1,1);
// draw models
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus1_drawn->Draw();
glPopMatrix();
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus2_drawn->Draw();
glPopMatrix();
break;
}
EndDraw();
}
template< typename Type > inline void glOneWire(Type & t, unsigned int a, unsigned int b) { glVertex3v(t->v[a]); glVertex3v(t->v[b]); }
void TraceVisRep::render(const shared_ptr<Node>& n, const GLViewInfo* glInfo){
if(isHidden() || !tracer) return;
if(!tracer->glSmooth) glDisable(GL_LINE_SMOOTH);
else glEnable(GL_LINE_SMOOTH);
glDisable(GL_LIGHTING);
bool scale=(glInfo->renderer->dispScale!=Vector3r::Ones() && glInfo->renderer->scaleOn && n->hasData<GlData>());
glLineWidth(tracer->glWidth);
const bool periodic=glInfo->scene->isPeriodic;
Vector3i prevPeriod=Vector3i::Zero(); // silence gcc warning maybe-uninitialized
int nSeg=0; // number of connected vertices (used when trace is interruped by NaN)
glBegin(GL_LINE_STRIP);
for(size_t i=0; i<pts.size(); i++){
size_t ix;
// FIXME: use getPointData here (copied code)
// compressed start from the very beginning, till they reach the write position
if(flags&FLAG_COMPRESS){
ix=i;
if(ix>=writeIx) break;
// cycle through the array
} else {
ix=(writeIx+i)%pts.size();
}
if(!isnan(pts[ix][0])){
Vector3r color;
if(isnan(scalars[ix])){
// if there is no scalar and no scalar should be saved, color by history position
if(tracer->scalar==Tracer::SCALAR_NONE) color=tracer->lineColor->color((flags&FLAG_COMPRESS ? i*1./writeIx : i*1./pts.size()));
// if other scalars are saved, use noneColor to not destroy tracer->lineColor range by auto-adjusting to bogus
else color=tracer->noneColor;
}
else color=tracer->lineColor->color(scalars[ix]);
if(isnan(color.maxCoeff())){
if(nSeg>0){ glEnd(); glBegin(GL_LINE_STRIP); nSeg=0; } // break line, if there was something already
continue; // point skipped completely
}
glColor3v(color);
Vector3r pt(pts[ix]);
if(periodic){
// canonicalize the point, store the period
Vector3i currPeriod;
pt=glInfo->scene->cell->canonicalizePt(pt,currPeriod);
// if the period changes between these two points, split the line (and don't render the segment in-between for simplicity)
if(i>0 && currPeriod!=prevPeriod){
if(nSeg>0){ glEnd(); glBegin(GL_LINE_STRIP); nSeg=0; } // only if there was sth already
// point not skipped, only line interruped, so keep going
}
prevPeriod=currPeriod;
}
if(!scale) glVertex3v(pt);
else{
const auto& gl=n->getData<GlData>();
// don't scale if refpos is invalid
if(isnan(gl.refPos.maxCoeff())) glVertex3v(pt);
// x+(s-1)*(x-x0)
else glVertex3v((pt+((glInfo->renderer->dispScale-Vector3r::Ones()).array()*(pt-gl.refPos).array()).matrix()).eval());
}
nSeg++;
}
}
glEnd();
}
void GLUtils::Parallelepiped(const Vector3r& a, const Vector3r& b, const Vector3r& c){
glBegin(GL_LINE_STRIP);
glVertex3v(b); glVertex3v(Vector3r(Vector3r::Zero())); glVertex3v(a); glVertex3v(Vector3r(a+b)); glVertex3v(Vector3r(a+b+c)); glVertex3v(Vector3r(b+c)); glVertex3v(b); glVertex3v(Vector3r(a+b));
glEnd();
glBegin(GL_LINE_STRIP);
glVertex3v(Vector3r(b+c)); glVertex3v(c); glVertex3v(Vector3r(a+c)); glVertex3v(a);
glEnd();
glBegin(GL_LINES);
glVertex3v(Vector3r(Vector3r::Zero())); glVertex3v(c);
glEnd();
glBegin(GL_LINES);
glVertex3v(Vector3r(a+c)); glVertex3v(Vector3r(a+b+c));
glEnd();
}
void drawPoint(const Vector3& pt)
{
glBegin(GL_POINTS);
glVertex3v(pt);
glEnd();
}
void Gl1_Membrane::go(const shared_ptr<Shape>& sh, const Vector3r& shift, bool wire2, const GLViewInfo& viewInfo){
Gl1_Facet::go(sh,shift,/*don't force wire rendering*/ wire2,viewInfo);
if(viewInfo.renderer->fastDraw) return;
Membrane& ff=sh->cast<Membrane>();
if(!ff.hasRefConf()) return;
if(node){
viewInfo.renderer->setNodeGlData(ff.node);
viewInfo.renderer->renderRawNode(ff.node);
if(ff.node->rep) ff.node->rep->render(ff.node,&viewInfo);
#ifdef MEMBRANE_DEBUG_ROT
// show what Membrane thinks the orientation of nodes is - render those midway
if(ff.currRot.size()==3){
for(int i:{0,1,2}){
shared_ptr<Node> n=make_shared<Node>();
n->pos=ff.node->pos+.5*(ff.nodes[i]->pos-ff.node->pos);
n->ori=(ff.currRot[i]*ff.node->ori.conjugate()).conjugate();
viewInfo.renderer->renderRawNode(n);
}
}
#endif
}
// draw everything in in local coords now
glPushMatrix();
if(!viewInfo.renderer->scaleOn){
// without displacement scaling, local orientation is easy
GLUtils::setLocalCoords(ff.node->pos,ff.node->ori);
} else {
/* otherwise compute scaled orientation such that
* +x points to the same point on the triangle (in terms of angle)
* +z is normal to the facet in the GL space
*/
Real phi0=atan2(ff.refPos[1],ff.refPos[0]);
Vector3r C=ff.getGlCentroid();
Matrix3r rot;
rot.row(2)=ff.getGlNormal();
rot.row(0)=(ff.getGlVertex(0)-C).normalized();
rot.row(1)=rot.row(2).cross(rot.row(0));
Quaternionr ori=AngleAxisr(phi0,Vector3r::UnitZ())*Quaternionr(rot);
GLUtils::setLocalCoords(C,ori.conjugate());
}
if(refConf){
glColor3v(refColor);
glLineWidth(refWd);
glBegin(GL_LINE_LOOP);
for(int i:{0,1,2}) glVertex3v(Vector3r(ff.refPos[2*i],ff.refPos[2*i+1],0));
glEnd();
}
if(uScale!=0){
glLineWidth(uWd);
for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),uScale*ff.uXy.segment<2>(2*i),uRange,uSplit,arrows?1:0,uWd);
}
if(relPhi!=0 && ff.hasBending()){
glLineWidth(phiWd);
for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),relPhi*viewInfo.sceneRadius*ff.phiXy.segment<2>(2*i),phiRange,phiSplit,arrows?2:0,phiWd
#ifdef MEMBRANE_DEBUG_ROT
, relPhi*viewInfo.sceneRadius*ff.drill[i] /* show the out-of-plane component */
#endif
);
}
glPopMatrix();
}
inline void glVertex( const QVector3D & v ) { glVertex3v( reinterpret_cast<const float*>(&v) ); }
void
DisplayCB()
{
BeginDraw();
// set up model transformations
if (animate)
{
rot1 += .1;
rot2 += .2;
rot3 += .3;
}
PQP_REAL R1[3][3],R2[3][3],T1[3],T2[3];
PQP_REAL M1[3][3],M2[3][3],M3[3][3];
T1[0] = -1;
T1[1] = 0.0;
T1[2] = 0.0;
T2[0] = 1;
T2[1] = 0.0;
T2[2] = 0.0;
MRotX(M1,rot1);
MRotY(M2,rot2);
MxM(M3,M1,M2);
MRotZ(M1,rot3);
MxM(R1,M3,M1);
MRotX(M1,rot3);
MRotY(M2,rot1);
MxM(M3,M1,M2);
MRotZ(M1,rot2);
MxM(R2,M3,M1);
// perform distance query
PQP_REAL rel_err = 0.0;
PQP_REAL abs_err = 0.0;
PQP_DistanceResult res;
PQP_Distance(&res,R1,T1,&bunny,R2,T2,&torus,rel_err,abs_err);
// draw the models
glColor3d(0.0,0.0,1.0);
double oglm[16];
MVtoOGL(oglm,R1,T1);
glPushMatrix();
glMultMatrixd(oglm);
bunny_to_draw->Draw();
glPopMatrix();
glColor3d(0.0,1.0,0.0);
MVtoOGL(oglm,R2,T2);
glPushMatrix();
glMultMatrixd(oglm);
torus_to_draw->Draw();
glPopMatrix();
// draw the closest points as small spheres
glColor3d(1.0,0.0,0.0);
PQP_REAL P1[3],P2[3],V1[3],V2[3];
VcV(P1,res.P1());
VcV(P2,res.P2());
// each point is in the space of its model;
// transform to world space
MxVpV(V1,R1,P1,T1);
/*
glPushMatrix();
glTranslated(V1[0],V1[1],V1[2]);
glutSolidSphere(.05,15,15);
glPopMatrix();
*/
MxVpV(V2,R2,P2,T2);
/*
glPushMatrix();
glTranslated(V2[0],V2[1],V2[2]);
glutSolidSphere(.05,15,15);
glPopMatrix();
*/
// draw the line between the closest points
float v1p[3], v2p[3];
for (int i = 0; i < 3; ++i) {
v1p[i] = V1[i];
v2p[i] = V2[i];
}
//glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3v(v1p);
glVertex3v(v2p);
glEnd();
//glEnable(GL_LIGHTING);
EndDraw();
}
void RobotTestBackend::RenderWorld()
{
ViewRobot& viewRobot = world->robotViews[0];
//WorldViewProgram::RenderWorld();
glDisable(GL_LIGHTING);
drawCoords(0.1);
glEnable(GL_LIGHTING);
for(size_t i=0;i<world->terrains.size();i++)
world->terrains[i]->DrawGL();
for(size_t i=0;i<world->rigidObjects.size();i++)
world->rigidObjects[i]->DrawGL();
if(draw_sensors) {
if(robotSensors.sensors.empty()) {
robotSensors.MakeDefault(robot);
}
for(size_t i=0;i<robotSensors.sensors.size();i++) {
vector<double> measurements;
if(0 == strcmp(robotSensors.sensors[i]->Type(),"CameraSensor")) {
robotSensors.sensors[i]->SimulateKinematic(*robot,*world);
robotSensors.sensors[i]->GetMeasurements(measurements);
}
robotSensors.sensors[i]->DrawGL(*robot,measurements);
}
}
if(draw_geom) {
//set the robot colors
GLColor highlight(1,1,0);
GLColor driven(1,0.5,0);
GLColor colliding(1,0,0);
GLColor blue(0,0,1);
viewRobot.RestoreAppearance();
viewRobot.PushAppearance();
for(size_t i=0;i<robot->links.size();i++) {
if(self_colliding[i]) viewRobot.SetColor(i,colliding);
if((int)i == cur_link)
viewRobot.SetColor(i,highlight);
else if(cur_driver >= 0 && cur_driver < (int)robot->drivers.size() &&
robot->DoesDriverAffect(cur_driver,i))
viewRobot.SetColor(i,driven);
if(draw_self_collision_tests) {
//draw a little blue
if(robot->selfCollisions(i,cur_link) || robot->selfCollisions(cur_link,i) ) {
GLDraw::GeometryAppearance &app = viewRobot.Appearance(i);
app.ModulateColor(blue,0.5);
}
}
}
//this will set the hover colors
allWidgets.DrawGL(viewport);
//viewRobot.Draw();
viewRobot.PopAppearance();
}
else {
for(size_t i=0;i<robotWidgets.size();i++)
robotWidgets[i].linkPoser.draw = 0;
allWidgets.DrawGL(viewport);
for(size_t i=0;i<robotWidgets.size();i++)
robotWidgets[i].linkPoser.draw = 1;
}
if(draw_com) {
viewRobot.DrawCenterOfMass();
for(size_t i=0;i<robot->links.size();i++)
viewRobot.DrawLinkCenterOfMass(i,0.01);
}
if(draw_frame) {
viewRobot.DrawLinkFrames();
glDisable(GL_DEPTH_TEST);
glPushMatrix();
glMultMatrix((Matrix4)robot->links[cur_link].T_World);
drawCoords(0.2);
glPopMatrix();
glEnable(GL_DEPTH_TEST);
}
if(draw_self_collision_tests) {
glDisable(GL_LIGHTING);
glLineWidth(2.0);
glColor3f(1,0,0);
glBegin(GL_LINES);
for(size_t i=0;i<robot->links.size();i++) {
Vector3 comi = robot->links[i].T_World*robot->links[i].com;
for(size_t j=0;j<robot->links.size();j++) {
if(robot->selfCollisions(i,j)) {
Vector3 comj = robot->links[j].T_World*robot->links[j].com;
glVertex3v(comi);
glVertex3v(comj);
}
}
}
glEnd();
}
if(draw_bbs) {
for(size_t j=0;j<robot->geometry.size();j++) {
if(robot->IsGeometryEmpty(j)) continue;
Box3D bbox = robot->geometry[j]->GetBB();
Matrix4 basis;
bbox.getBasis(basis);
glColor3f(1,0,0);
drawOrientedWireBox(bbox.dims.x,bbox.dims.y,bbox.dims.z,basis);
}
}
}
