internal static void DrawCircle(GLPen circPen, Vector2 vector2, float p)
{
DrawCircle(circPen, vector2.ToPointF(), p);
}
public static void DrawPointList(GLPen pen, List<Vector3> pts)
{
DrawPointList(pen, pts, false);
}
public static void DrawRectangle(GLPen pen, RectangleF rect)
{
PointF p1 = new PointF (rect.Left, rect.Top);
PointF p2 = new PointF (rect.Right, rect.Top);
PointF p3 = new PointF(rect.Right, rect.Bottom);
PointF p4 = new PointF(rect.Left, rect.Bottom);
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINE_LOOP);
Gl.glVertex2f(p1.X, p1.Y);
Gl.glVertex2f(p2.X, p2.Y);
Gl.glVertex2f(p3.X, p3.Y);
Gl.glVertex2f(p4.X, p4.Y);
Gl.glEnd();
}
public static void DrawLines(GLPen pen, Vector2[] points)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINE_STRIP);
foreach (Vector2 p in points)
Gl.glVertex2d(p.X, p.Y);
Gl.glEnd();
}
public static void DrawPoint(GLPen pen, Vector2 p, bool ApplyPen)
{
if (ApplyPen) pen.GLApplyPen();
Gl.glBegin(Gl.GL_POINT);
Gl.glVertex2f((float)p.X, (float)p.Y);
Gl.glEnd();
}
public static void DrawLine(GLPen pen, float x1, float y1, float x2, float y2)
{
DrawLine(pen, new PointF(x1, y1), new PointF(x2, y2));
}
public static void DrawLine3D(GLPen pen, Vector3 a, Vector3 b)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex3d(a.X, a.Y, a.Z);
Gl.glVertex3d(b.X, b.Y, b.Z);
Gl.glEnd();
}
public static void DrawCircle(GLPen p, PointF center, float radius)
{
p.GLApplyPen();
Gl.glPushMatrix();
Gl.glTranslatef(center.X, center.Y, 0);
GLUtility.DrawEllipse(p, new RectangleF(-radius, -radius, radius * 2, radius*2));
Gl.glPopMatrix();
}
public static void DrawCircle(GLPen p, PointF center, float radius, double thetaStart, double thetaEnd )
{
p.GLApplyPen();
Gl.glPushMatrix();
Gl.glTranslatef(center.X, center.Y, 0);
RectangleF rect = new RectangleF(-radius, -radius, radius * 2, radius * 2);
Gl.glPushMatrix();
Gl.glTranslatef(rect.Left+rect.Width /2, rect.Top + rect.Height /2 , 0);
Gl.glBegin(Gl.GL_LINE_STRIP);
if (thetaStart < thetaEnd)
{
for (double i = thetaStart; i < thetaEnd; i += 0.05)
Gl.glVertex2f((float)(Math.Cos(i) * rect.Width / 2), (float)(Math.Sin(i) * rect.Height / 2));
}
Gl.glEnd();
Gl.glPopMatrix();
Gl.glPopMatrix();
}
public static void DrawBox(PointF p, float h, float width, float length, string label, bool rotate90, DrawRef drawRef, Color color)
{
GLUtility.GoToVehicleCoordinates(h, p, rotate90);
RectangleF r = new RectangleF();
if (drawRef == DrawRef.RearAxle)
r = new RectangleF(-width / 2, 0, width, length);
else if (drawRef == DrawRef.Center)
r = new RectangleF(-width / 2, -length / 2, width, length);
else if (drawRef == DrawRef.BottomCorner)
r = new RectangleF(0, 0, width, length);
GLPen pen = new GLPen(color, 1.0f);
GLUtility.DrawRectangle(pen, r);
GLUtility.DrawLine(pen, r.Left + 0.2f, r.Bottom - r.Width / 2, r.Left + r.Width / 2, r.Bottom - .2f);
GLUtility.DrawLine(pen, r.Right - 0.2f, r.Bottom - r.Width / 2, r.Right - r.Width / 2, r.Bottom - .2f);
if (label != "") GLUtility.DrawString(label, new Font("verdana", 1.0f), Color.Black, r.Location);
GLUtility.ComeBackFromVehicleCoordinates();
}
/* public static void DrawCar(PointF rearAxle, float h, int txid)
{
//the rear axle point is just the center of the car in x and the rear of the car in y
PointF carPoint = new PointF(rearAxle.X - (carWidth / 2), rearAxle.Y);
GLUtility.GoToVehicleCoordinates(h, carPoint);
RectangleF r = new RectangleF(0, 0, carWidth, carHeight);
GLPen pen = new GLPen(Color.LightBlue, 1.0f);
GLUtility.FillTexturedRectangle(txid, r);
GLUtility.ComeBackFromVehicleCoordinates();
}
*/
//0 degree heading means positive X and means EAST
public static void DrawCar(PointF rearAxle, float h, Color color, string label, bool rotate90, Color labelColor, float width, float centerpointToFront, float height)
{
float carWidth = width;
float carIMUtoFront = centerpointToFront;
float carHeight = height;
//the rear axle point is just the center of the car in x and the rear of the car in y
PointF IMU = new PointF (rearAxle.X,rearAxle.Y);
GLUtility.GoToVehicleCoordinates(h, IMU,rotate90);
RectangleF r = new RectangleF(-carWidth / 2, carIMUtoFront - carHeight, carWidth, carHeight);
GLPen pen = new GLPen(color, 1.0f);
GLUtility.DrawRectangle(pen, r);
GLUtility.DrawLine(pen, r.Left + 0.2f, r.Bottom - r.Width / 2, r.Left + r.Width / 2, r.Bottom - .2f);
GLUtility.DrawLine(pen, r.Right - 0.2f, r.Bottom - r.Width / 2, r.Right - r.Width / 2, r.Bottom - .2f);
if (label != "") GLUtility.DrawString(label, new Font("verdana", 1.0f), labelColor, new PointF(+.20f, 0));
GLUtility.DrawCross(new GLPen (Color.Pink,1.0f), new Vector2(0, 0), r.Width / 2);
GLUtility.ComeBackFromVehicleCoordinates();
}
public static void DrawBezier(GLPen pen, Vector2 startP, Vector2 ctrl1, Vector2 ctrl2, Vector2 endP)
{
CubicBezier cb = new CubicBezier(startP, ctrl1, ctrl2, endP);
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINE_STRIP);
//iterate this bitch
for (double i = 0; i < 1.0; i += .025)
{
Vector2 p = (cb.Bt(i));
Gl.glVertex2f((float)p.X, (float)p.Y);
}
Vector2 p1 = (cb.Bt(1));
Gl.glVertex2f((float)p1.X, (float)p1.Y);
Gl.glEnd();
}
public static void Draw3DRectangle(GLPen pen, Vector3 bl, Vector3 br, Vector3 tl, Vector3 tr)
{
pen.GLApplyPen();
Gl.glShadeModel(Gl.GL_SMOOTH);
//Gl.glColor4ub(color.R, color.G, color.B, color.A);
Gl.glBegin(Gl.GL_QUADS);
Gl.glVertex3f((float)tl.X, (float)tl.Y, (float)tl.Z);
Gl.glVertex3f((float)tr.X, (float)tr.Y, (float)tr.Z);
Gl.glVertex3f((float)br.X, (float)br.Y, (float)br.Z);
Gl.glVertex3f((float)bl.X, (float)bl.Y, (float)bl.Z);
Gl.glEnd();
}
internal static void DrawLine(GLPen pen, PointF p1, PointF p2)
{
DrawLine(pen, Vector2.FromPointF(p1), Vector2.FromPointF(p2));
}
public static void DrawEllipse(GLPen p, Vector2 firstPoint, Vector2 secondPoint, Vector2 thirdPoint)
{
float height = (float)firstPoint.DistanceTo(thirdPoint);
DrawEllipse(p, firstPoint, secondPoint, height);
//Vector2 delta = new Vector2(secondPDFPoint.X - firstPDFPoint.X, secondPDFPoint.Y - firstPDFPoint.Y);
//double thirdPointAngle = delta.ToRadians();
//thirdPDFPoint = new Vector2(mu.X + Math.Sin(thirdPointAngle) * height2, mu.Y - Math.Cos(thirdPointAngle) * height2);
}
public static void DrawCluster(PointF p, float h, float width, string label, bool rotate90)
{
GLUtility.GoToVehicleCoordinates(h, p, rotate90);
GLPen pen = new GLPen(Color.Blue, 1.0f);
GLUtility.DrawLine(pen,-width/2,0,width/2,0);
GLUtility.DrawLine(pen, 0, .1f, 0, -.1f);
if (label != "") GLUtility.DrawString(label, new Font("verdana", 1.0f), Color.Black, new PointF(0,0));
GLUtility.ComeBackFromVehicleCoordinates();
}
public static void DrawGrid3D(int gridStep, WorldTransform w, float z, Color lineColor, Color textColor, Color radiiColor)
{
GLUtility.DisableNiceLines();
GLPen pen = new GLPen(lineColor, 1.0f);
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINES);
for (int i = (int)w.WorldLowerLeft.Y; i < w.WorldUpperRight.Y; i++)
{
if (i % gridStep == 0)
{
Gl.glVertex3f((float)w.WorldLowerLeft.X, i, z);
Gl.glVertex3f((float)w.WorldUpperRight.X, i, z);
//GLUtility.DrawString(i.ToString() + "m", new Font("verdana", 1), textColor, new PointF(0, i + .25f));
}
}
for (int i = (int)w.WorldLowerLeft.X; i < w.WorldUpperRight.X; i++)
{
if (i % gridStep == 0)
{
Gl.glVertex3f(i,(float)w.WorldLowerLeft.Y,z);
Gl.glVertex3f(i, (float)w.WorldUpperRight.Y, z);
//GLUtility.DrawString(i.ToString() + "m", new Font("verdana", 1), textColor, new PointF(i, (float)(w.WorldUpperRight.Y - w.WorldLowerLeft.Y) / 2.0f));
}
}
Gl.glEnd();
GLUtility.EnableNiceLines();
}
public static void DrawCross3D(GLPen pen, Vector3 p, float size)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex3d(p.X + size, p.Y, p.Z);
Gl.glVertex3d(p.X - size, p.Y, p.Z);
Gl.glVertex3d(p.X, p.Y + size, p.Z);
Gl.glVertex3d(p.X, p.Y - size, p.Z);
Gl.glVertex3d(p.X, p.Y, p.Z + size);
Gl.glVertex3d(p.X, p.Y, p.Z - size);
Gl.glEnd();
}
public static void DrawLine(GLPen pen, Vector2 p1, Vector2 p2)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINE_STRIP);
Gl.glVertex2f((float)p1.X, (float)p1.Y);
Gl.glVertex2f((float)p2.X, (float)p2.Y);
Gl.glEnd();
}
public static void DrawCube(GLPen pen, Vector3 p, float size)
{
pen.GLApplyPen();
Gl.glPushMatrix();
Gl.glTranslated(p.X, p.Y, p.Z);
Gl.glScalef(size, size, size);
Gl.glBegin(Gl.GL_QUADS);
// Front Face
Gl.glNormal3f(0.0f, 0.0f, 1.0f);
Gl.glVertex3f(-1.0f, -1.0f, 1.0f);
Gl.glVertex3f(1.0f, -1.0f, 1.0f);
Gl.glVertex3f(1.0f, 1.0f, 1.0f);
Gl.glVertex3f(-1.0f, 1.0f, 1.0f);
// Back Face
Gl.glNormal3f(0.0f, 0.0f, -1.0f);
Gl.glVertex3f(-1.0f, -1.0f, -1.0f);
Gl.glVertex3f(-1.0f, 1.0f, -1.0f);
Gl.glVertex3f(1.0f, 1.0f, -1.0f);
Gl.glVertex3f(1.0f, -1.0f, -1.0f);
// Top Face
Gl.glNormal3f(0.0f, 1.0f, 0.0f);
Gl.glVertex3f(-1.0f, 1.0f, -1.0f);
Gl.glVertex3f(-1.0f, 1.0f, 1.0f);
Gl.glVertex3f(1.0f, 1.0f, 1.0f);
Gl.glVertex3f(1.0f, 1.0f, -1.0f);
// Bottom Face
Gl.glNormal3f(0.0f, -1.0f, 0.0f);
Gl.glVertex3f(-1.0f, -1.0f, -1.0f);
Gl.glVertex3f(1.0f, -1.0f, -1.0f);
Gl.glVertex3f(1.0f, -1.0f, 1.0f);
Gl.glVertex3f(-1.0f, -1.0f, 1.0f);
// Right face
Gl.glNormal3f(1.0f, 0.0f, 0.0f);
Gl.glVertex3f(1.0f, -1.0f, -1.0f);
Gl.glVertex3f(1.0f, 1.0f, -1.0f);
Gl.glVertex3f(1.0f, 1.0f, 1.0f);
Gl.glVertex3f(1.0f, -1.0f, 1.0f);
// Left Face
Gl.glNormal3f(-1.0f, 0.0f, 0.0f);
Gl.glVertex3f(-1.0f, -1.0f, -1.0f);
Gl.glVertex3f(-1.0f, -1.0f, 1.0f);
Gl.glVertex3f(-1.0f, 1.0f, 1.0f);
Gl.glVertex3f(-1.0f, 1.0f, -1.0f);
Gl.glEnd();
Gl.glPopMatrix();
}
public static void DrawLine3D(GLPen pen, float aX, float aY, float aZ, float bX, float bY, float bZ)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex3f(aX, aY, aZ);
Gl.glVertex3f(bX, bY, bZ);
Gl.glEnd();
}
public static void DrawDiamond(GLPen pen, Vector2 p, float size)
{
Vector2 p1 = new Vector2(p.X, p.Y + size / 2.0f);
Vector2 p2 = new Vector2(p.X, p.Y - size / 2.0f);
Vector2 p3 = new Vector2(p.X + size / 2.0f, p.Y);
Vector2 p4 = new Vector2(p.X - size / 2.0f, p.Y);
DrawLine(pen, p1, p3);
DrawLine(pen, p3, p2);
DrawLine(pen, p2, p4);
DrawLine(pen, p4, p1);
}
public static void DrawPoint(GLPen pen, Vector2 p)
{
DrawPoint(pen, p, true);
}
public static void DrawEllipse(GLPen p, float x, float y, float width, float height)
{
DrawEllipse(p, new RectangleF(x, y, width, height));
}
public static void DrawPoint3D(GLPen pen, Vector3 p)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_POINT);
Gl.glVertex3d(p.X, p.Y, p.Z);
Gl.glEnd();
}
public static void DrawEllipse(GLPen p, RectangleF rect)
{
p.GLApplyPen();
Gl.glPushMatrix();
Gl.glTranslatef(rect.Left+rect.Width /2, rect.Top + rect.Height /2 , 0);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (double i = 0; i < Math.PI * 2; i += 0.05)
Gl.glVertex2f((float)(Math.Cos(i) * rect.Width/2), (float)(Math.Sin(i) * rect.Height/2));
Gl.glEnd();
Gl.glPopMatrix();
}
public static void DrawPointList(GLPen pen, List<Vector3> pts, bool zEqualsZero)
{
Color color = pen.color;
Gl.glBegin(Gl.GL_POINTS);
foreach (Vector3 vp in pts)
{
Gl.glColor4ub(color.R, color.G, color.B, color.A);
float[] c = { (float)color.R / 255.0f, (float)color.G / 255.0f, (float)color.B / 255.0f, (float)color.A / 255.0f };
Gl.glMaterialfv(Gl.GL_FRONT_AND_BACK, Gl.GL_AMBIENT_AND_DIFFUSE, c);
Gl.glVertex3d(vp.X, vp.Y, zEqualsZero?0:vp.Z);
}
Gl.glEnd();
}
public static void DrawEllipse(GLPen p, Vector2 firstPoint, Vector2 secondPoint, float height)
{
Vector2 deltaPoint = new Vector2(firstPoint.X - secondPoint.X, firstPoint.Y - secondPoint.Y);
double angle = deltaPoint.ToRadians();
deltaPoint = deltaPoint.Rotate90();
//Vector2 thirdPoint = deltaPoint.Normalize() * height;
float length = (float)firstPoint.DistanceTo(secondPoint);
Vector2 PreRotDrawpoint = new Vector2(0, 0);
Vector2 PostRotDrawPoint = new Vector2(0, 0);
p.GLApplyPen();
Gl.glPushMatrix();
Gl.glTranslatef((float)firstPoint.X, (float)firstPoint.Y, 0);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (double i = 0; i < Math.PI * 2; i += 0.05)
{
PreRotDrawpoint = new Vector2((float)(Math.Cos(i) * length), (float)(Math.Sin(i) * height));
PostRotDrawPoint = PreRotDrawpoint.Rotate(angle);
Gl.glVertex2f((float)PostRotDrawPoint.X, (float)PostRotDrawPoint.Y);
}
Gl.glEnd();
Gl.glPopMatrix();
}
public static void DrawWireframeBox3D(Vector3 a, Vector3 b, GLPen pen)
{
pen.GLApplyPen();
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex3d(a.X, a.Y, a.Z);
Gl.glVertex3d(b.X, a.Y, a.Z);
Gl.glVertex3d(a.X, b.Y, a.Z);
Gl.glVertex3d(b.X, b.Y, a.Z);
Gl.glVertex3d(a.X, a.Y, b.Z);
Gl.glVertex3d(b.X, a.Y, b.Z);
Gl.glVertex3d(a.X, b.Y, b.Z);
Gl.glVertex3d(b.X, b.Y, b.Z);
Gl.glVertex3d(a.X, a.Y, a.Z);
Gl.glVertex3d(a.X, b.Y, a.Z);
Gl.glVertex3d(a.X, a.Y, b.Z);
Gl.glVertex3d(a.X, b.Y, b.Z);
Gl.glVertex3d(b.X, a.Y, a.Z);
Gl.glVertex3d(b.X, b.Y, a.Z);
Gl.glVertex3d(b.X, a.Y, b.Z);
Gl.glVertex3d(b.X, b.Y, b.Z);
Gl.glVertex3d(a.X, a.Y, a.Z);
Gl.glVertex3d(a.X, a.Y, b.Z);
Gl.glVertex3d(b.X, a.Y, a.Z);
Gl.glVertex3d(b.X, a.Y, b.Z);
Gl.glVertex3d(a.X, b.Y, a.Z);
Gl.glVertex3d(a.X, b.Y, b.Z);
Gl.glVertex3d(b.X, b.Y, a.Z);
Gl.glVertex3d(b.X, b.Y, b.Z);
Gl.glEnd();
}
private void DrawLine(int i, int j)
{
double x, y;
GLPen pen;
float heightVal = (float)(grid.GetCellByIdx(i, j));
if (heightVal >= .01)
{
grid.GetReals(i, j, out x, out y);
pen = new GLPen(ColorFromHeight(heightVal), 20f);
pen.GLApplyPen();
Gl.glVertex3f((float)x, (float)y, heightVal);
Gl.glVertex3f((float)x, (float)y, 0);
}
}
