void drawCylinder(const Vector3& h,Real r,int steps) { int i; float inc = Two*Pi/steps; Real height=h.norm(); Vector3 hdir=h/height; Vector3 xb,yb; GetCanonicalBasis(hdir,xb,yb); Complex x,dx; dx.setPolar(1,inc); Vector3 point,point2; Vector3 n,n2; //base glBegin(GL_TRIANGLE_FAN); glNormal3v(-hdir); glVertex3f(0,0,0); x.set(r,0); for(i=0; i<=steps; i++) { point = xb*(-x.x) + yb*x.y; glVertex3v(point); x = x*dx; } glEnd(); //cap glBegin(GL_TRIANGLE_FAN); glNormal3v(hdir); glVertex3v(h); x.set(r,0); for(i=0; i<=steps; i++) { point = xb*(x.x) + yb*(-x.y) + h; glVertex3v(point); x = x*dx; } glEnd(); //sides glBegin(GL_TRIANGLE_STRIP); dx.setPolar(1,-inc); x.set(r,0); for(i=0; i<=steps; i++) { point = xb*x.x + yb*x.y; n = (-x.y*xb + x.x*yb); n.inplaceNormalize(); glNormal3v(n); glVertex3v(point); glNormal3v(n); glVertex3v(point+h); x = x*dx; } glEnd(); }
Matrix3 InertiaMatrix(const GeometricPrimitive3D& geom,Real mass) { switch(geom.type) { case GeometricPrimitive3D::Empty: case GeometricPrimitive3D::Point: return Matrix3(0.0); case GeometricPrimitive3D::Segment: { Matrix3 Ilocal; const Segment3D* s=AnyCast_Raw<Segment3D>(&geom.data); Real len=s->a.distance(s->b); CylinderInertiaMatrix(0,len,mass,Ilocal); if(len==0) return Ilocal; Vector3 x,y,z; x=(s->b-s->a)/len; GetCanonicalBasis(x,y,z); Matrix3 R(x,y,z),I; I.mulTransposeB(R*Ilocal,R); return I; } case GeometricPrimitive3D::Sphere: { Matrix3 I; SphereInertiaMatrix(AnyCast_Raw<Sphere3D>(&geom.data)->radius,mass,I); return I; } case GeometricPrimitive3D::AABB: { Vector3 dims = AnyCast_Raw<AABB3D>(&geom.data)->bmax-AnyCast_Raw<AABB3D>(&geom.data)->bmin; Matrix3 I; BoxInertiaMatrix(dims.x,dims.y,dims.z,mass,I); return I; } case GeometricPrimitive3D::Box: { const Box3D* b=AnyCast_Raw<Box3D>(&geom.data); Matrix3 Ilocal,I; BoxInertiaMatrix(b->dims.x,b->dims.y,b->dims.z,mass,Ilocal); RigidTransform T = geom.GetFrame(); I.mulTransposeB(T.R*I,T.R); return I; } case GeometricPrimitive3D::Ellipsoid: { const Ellipsoid3D* b=AnyCast_Raw<Ellipsoid3D>(&geom.data); Matrix3 Ilocal,I; EllipsoidInertiaMatrix(b->dims.x,b->dims.y,b->dims.z,mass,Ilocal); RigidTransform T = geom.GetFrame(); I.mulTransposeB(T.R*I,T.R); return I; } default: FatalError("Can't do inertia for that geom type yet"); return Matrix3(); } }
void drawCone(const Vector3& h, float r, int steps) { int i; float inc = fTwoPi/steps; Real height=h.norm(); Vector3 hdir=h/height; Vector3 xb,yb; GetCanonicalBasis(hdir,xb,yb); Complex x,dx; dx.setPolar(1,inc); Vector3 point,point2; Vector3 n,n2; //base glBegin(GL_TRIANGLE_FAN); glNormal3v(-hdir); glVertex3f(0,0,0); x.set(r,0); for(i=0; i<=steps; i++) { point = xb*(-x.x) + yb*x.y; glVertex3v(point); x = x*dx; } glEnd(); //sides glBegin(GL_TRIANGLES); x.set(r,0); for(i=0; i<steps; i++) { point = xb*x.x + yb*x.y; n = (-x.y*xb + x.x*yb)/r*height + r*hdir; n.inplaceNormalize(); x = x*dx; point2 = xb*x.x + yb*x.y; n2 = (-x.y*xb + x.x*yb)/r*height + r*hdir; n2.inplaceNormalize(); glNormal3v(hdir); glVertex3v(h); glNormal3v(n); glVertex3v(point); glNormal3v(n2); glVertex3v(point2); } glEnd(); }
Real HollowBall::closestPoint(const AxisSweptPoint& p) const { Vector3 x,y; GetCanonicalBasis(p.axis.direction,x,y); Vector2 c2,p2,s2; c2.x = x.dot(center); c2.y = y.dot(center); p2.x = x.dot(p.p); p2.y = y.dot(p.p); s2.x = x.dot(p.axis.source); s2.y = y.dot(p.axis.source); c2-=s2; p2-=s2; Real ac = Atan2(c2.y,c2.x); Real ap = Atan2(p2.y,p2.x); return ac-ap; }
void drawWireCircle(const Vector3& axis,float r, int numIncrements) { float inc = fTwoPi/numIncrements; int i; Vector3 u,v; GetCanonicalBasis(axis,u,v); Complex x,dx; dx.setPolar(One,inc); glBegin(GL_LINE_LOOP); x.set(r,0); for(i=0; i<numIncrements; i++) { glVertex3v(x.x*u+x.y*v); x=x*dx; } glEnd(); }
void drawWireCylinder(const Vector3& h,Real r,int numSteps) { Real len = h.length(); Vector3 axis = h/len; float inc = fTwoPi/numSteps; int i; Vector3 u,v; GetCanonicalBasis(axis,u,v); Complex x,dx; dx.setPolar(One,inc); glBegin(GL_LINE_LOOP); x.set(r,0); for(i=0; i<numSteps; i++) { glVertex3v(x.x*u+x.y*v); x=x*dx; } glEnd(); glBegin(GL_LINE_LOOP); x.set(r,0); for(i=0; i<numSteps; i++) { glVertex3v(h + x.x*u+x.y*v); x=x*dx; } glEnd(); glBegin(GL_LINES); x.set(r,0); for(i=0; i<numSteps; i++) { glVertex3v(x.x*u+x.y*v); glVertex3v(h + x.x*u+x.y*v); x=x*dx; } glEnd(); }
void drawWireConeFlipped(const Vector3& h, float r, int steps) { int i; float inc = fTwoPi/steps; Vector3 xb,yb; Vector3 hdir=h; hdir.inplaceNormalize(); GetCanonicalBasis(hdir,xb,yb); Vector3 point; Complex x,dx; dx.setPolar(1,inc); glBegin(GL_LINES); x.set(r,0); for(i=0; i<steps; i++) { point = xb*x.x + yb*x.y + h; glVertex3f(0,0,0); glVertex3v(point); x = x*dx; } glEnd(); glBegin(GL_LINE_LOOP); x.set(r,0); for(i=0; i<steps; i++) { point = xb*x.x + yb*x.y + h; glVertex3v(point); x = x*dx; } glEnd(); }