public v3f cross(v3f b) { v3f res; res.x = y * b.z - z * b.y; res.y = z * b.x - x * b.z; res.z = x * b.y - y * b.x; return res; }
public v3f rotateAbout(v3f axis, float rotationRad) { v3f res = new v3f(); v3f uvw = axis.norm(); //http://www.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.htm float u = uvw.x; float v = uvw.y; float w = uvw.z; float SIN = (float)Math.Sin(rotationRad); float COS = (float)Math.Cos(rotationRad); float u2 = u * u; float v2 = v * v; float w2 = w * w; float ux = u * x; float vy = v * y; float wz = w * z; float dot = ux + vy + wz; // x=u(ux+vy+wz)+(x(v*v+w*w)+u(-vy-wz))cos(theta)+mag(uvw)(vz-wy)sin(theta) res.x = u * dot + COS * (x * (v2 + w2) + u * (-vy - wz)) + uvw.mag() * (v * z - w * y) * SIN; res.y = v * dot + COS * (y * (u2 + w2) + v * (-ux - wz)) + uvw.mag() * (w * x - u * z) * SIN; res.z = w * dot + COS * (z * (u2 + v2) + w * (-ux - vy)) + uvw.mag() * (u * y - v * x) * SIN; return res; }
private void ParseClustersPacket(BinaryReader br) { ClusterMsg msg = new ClusterMsg (); // read the header byte packetType = br.ReadByte(); if (packetType != CLUSTER_PACKET_TYPE) { Debug.WriteLine("ERROR: cluster parser got packet starting with " + packetType.ToString()); return; } UInt16 packetNum = br.ReadUInt16(); UInt16 tsSeconds = br.ReadUInt16(); UInt32 tsTicks = br.ReadUInt32(); UInt16 numPts = br.ReadUInt16(); UInt16 numClusters = br.ReadUInt16(); msg.timestamp = (double)tsSeconds + (double)tsTicks / 10000.0; // now for the red meat.. v3f[] pts = new v3f[numPts]; for (int i = 0; i < numPts; i++) { pts[i].x = (float)(br.ReadInt16())/100.0f; pts[i].y = (float)(br.ReadInt16())/100.0f; pts[i].z = (float)(br.ReadInt16())/100.0f; } msg.clusters = new List<LidarCluster>(); for (int i = 0; i < numClusters; i++) { LidarCluster lc = new LidarCluster(); UInt16 firstPtIndex, lastPtIndex; byte flags; firstPtIndex = br.ReadUInt16(); lastPtIndex = br.ReadUInt16(); flags = br.ReadByte(); lc.pts = new v3f[lastPtIndex - firstPtIndex + 1]; lc.stable = (flags & 0x01)==0; lc.leftOccluded = (flags & 0x02)!=0; lc.rightOccluded = (flags & 0x04)!=0; lc.highObstacle = (flags & 0x08)!=0; lc.bb_low.x = lc.bb_high.x = pts[firstPtIndex].x; lc.bb_low.y = lc.bb_high.y = pts[firstPtIndex].y; lc.bb_low.z = lc.bb_high.z = pts[firstPtIndex].z; for (int j = firstPtIndex; j <= lastPtIndex; j++) { lc.pts[j - firstPtIndex] = pts[j]; lc.bb_low.x = Math.Min(lc.bb_low.x, pts[j].x); lc.bb_low.y = Math.Min(lc.bb_low.y, pts[j].y); lc.bb_low.z = Math.Min(lc.bb_low.z, pts[j].z); lc.bb_high.x = Math.Max(lc.bb_high.x, pts[j].x); lc.bb_high.y = Math.Max(lc.bb_high.y, pts[j].y); lc.bb_high.z = Math.Max(lc.bb_high.z, pts[j].z); } msg.clusters.Add(lc); } if (ClustersReceived != null) ClustersReceived(this, new ClusterRXEventArgs(msg)); }
public GLCameraChase(WorldTransform wt) { eye = eyeRel; center = centerRel; up = new v3f(0, 0, 1); UpdateWithWorldTransform(wt); }
public static void DrawWireframeBox3D(v3f a, v3f b, GLPen pen) { pen.GLApplyPen(); Gl.glBegin(Gl.GL_LINES); Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(b.x, a.y, a.z); Gl.glVertex3f(a.x, b.y, a.z); Gl.glVertex3f(b.x, b.y, a.z); Gl.glVertex3f(a.x, a.y, b.z); Gl.glVertex3f(b.x, a.y, b.z); Gl.glVertex3f(a.x, b.y, b.z); Gl.glVertex3f(b.x, b.y, b.z); Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(a.x, b.y, a.z); Gl.glVertex3f(a.x, a.y, b.z); Gl.glVertex3f(a.x, b.y, b.z); Gl.glVertex3f(b.x, a.y, a.z); Gl.glVertex3f(b.x, b.y, a.z); Gl.glVertex3f(b.x, a.y, b.z); Gl.glVertex3f(b.x, b.y, b.z); Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(a.x, a.y, b.z); Gl.glVertex3f(b.x, a.y, a.z); Gl.glVertex3f(b.x, a.y, b.z); Gl.glVertex3f(a.x, b.y, a.z); Gl.glVertex3f(a.x, b.y, b.z); Gl.glVertex3f(b.x, b.y, a.z); Gl.glVertex3f(b.x, b.y, b.z); Gl.glEnd(); }
public static void DrawWireframeBox3D_glVertexOnly(v3f a, v3f b) { Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(b.x, a.y, a.z); Gl.glVertex3f(a.x, b.y, a.z); Gl.glVertex3f(b.x, b.y, a.z); Gl.glVertex3f(a.x, a.y, b.z); Gl.glVertex3f(b.x, a.y, b.z); Gl.glVertex3f(a.x, b.y, b.z); Gl.glVertex3f(b.x, b.y, b.z); Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(a.x, b.y, a.z); Gl.glVertex3f(a.x, a.y, b.z); Gl.glVertex3f(a.x, b.y, b.z); Gl.glVertex3f(b.x, a.y, a.z); Gl.glVertex3f(b.x, b.y, a.z); Gl.glVertex3f(b.x, a.y, b.z); Gl.glVertex3f(b.x, b.y, b.z); Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(a.x, a.y, b.z); Gl.glVertex3f(b.x, a.y, a.z); Gl.glVertex3f(b.x, a.y, b.z); Gl.glVertex3f(a.x, b.y, a.z); Gl.glVertex3f(a.x, b.y, b.z); Gl.glVertex3f(b.x, b.y, a.z); Gl.glVertex3f(b.x, b.y, b.z); }
public static void DrawLine3D(GLPen pen, v3f a, v3f b) { pen.GLApplyPen(); Gl.glBegin(Gl.GL_LINES); Gl.glVertex3f(a.x, a.y, a.z); Gl.glVertex3f(b.x, b.y, b.z); Gl.glEnd(); }
public static void DrawPoint3D(GLPen pen, v3f p) { pen.GLApplyPen(); Gl.glBegin(Gl.GL_POINT); //Gl.glVertex3f(p.x, p.y, 0.0f);//p.z); Gl.glVertex2f(p.x, p.y); Gl.glEnd(); }
public static void DrawCross3D(GLPen pen, v3f p, float size) { pen.GLApplyPen(); Gl.glBegin(Gl.GL_LINES); Gl.glVertex3f(p.x + size, p.y, p.z); Gl.glVertex3f(p.x - size, p.y, p.z); Gl.glVertex3f(p.x, p.y + size, p.z); Gl.glVertex3f(p.x, p.y - size, p.z); Gl.glVertex3f(p.x, p.y, p.z + size); Gl.glVertex3f(p.x, p.y, p.z - size); Gl.glEnd(); }
public static void DrawCube(GLPen pen, v3f p, float size) { pen.GLApplyPen(); Gl.glPushMatrix(); Gl.glTranslatef(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 void Pan(v3f v) { v3f lookat = new v3f(); lookat.z = -(float)Math.Sin(pitch / 180.0 * Math.PI); float horiz = (float)Math.Cos(pitch / 180.0 * Math.PI); lookat.x = horiz * (float)Math.Sin(yaw / 180.0 * Math.PI); lookat.y = horiz * (float)Math.Cos(yaw / 180.0 * Math.PI); v3f forward = lookat; v3f left = up.cross(forward).norm(); v3f T = (forward * v.x + left*v.y + up.norm()*v.z); location = location + T; }
public override void ApplyProjection() { v3f lookat = new v3f(); lookat.z = -(float)Math.Sin(pitch / 180.0 * Math.PI); float horiz = (float)Math.Cos(pitch / 180.0 * Math.PI); lookat.x = horiz*(float)Math.Sin(yaw / 180.0 * Math.PI); lookat.y = horiz*(float)Math.Cos(yaw / 180.0 * Math.PI); lookat = lookat + location; Glu.gluPerspective(45.0, aspect, .01, 1000); Glu.gluLookAt(location.x, location.y, location.z, lookat.x, lookat.y, lookat.z, up.x, up.y, up.z); }
float pitch, yaw; // both in degrees #endregion Fields #region Constructors public GLCameraFree(WorldTransform wt) { location = new v3f(-5.0f, -5, 5); up = new v3f(0, 0, 1); pitch = 30; yaw = 30; UpdateWithWorldTransform(wt); }
public void Pitch(float rotDegrees) { v3f v = up.cross(centerRel); v3f cv = centerRel - eyeRel; centerRel = cv.rotateAbout(v, rotDegrees / 180.0f * (float)Math.PI); }
public void MoveToPointAndHeading(PointF point, double rotRadians) { v3f vPoint = new v3f(point.X, point.Y, 1.0f); v3f forward = new v3f((float)Math.Cos(rotRadians), (float)Math.Sin(rotRadians), 0); float cDist = (float)Math.Sqrt(centerRel.x * centerRel.x + centerRel.y * centerRel.y); float cZ = centerRel.z; float eDist = (float)Math.Sqrt(eyeRel.x * eyeRel.x + eyeRel.y * eyeRel.y); float eZ = eyeRel.z; center = vPoint + forward * cDist; center.z = cZ; eye = vPoint - forward * eDist; eye.z = eZ; }