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();
}
