static Vector3 SamplePotential(Point3 p) { Vector3 psi(0,0,0); Vector3 gradient = ComputeGradient(p); float obstacleDistance = SampleDistance(p); // add turbulence octaves that respect boundaries, increasing upwards float height_factor = ramp((p.getY() - PlumeBase) / PlumeHeight); for (unsigned int i=0; i < 3; ++i) { Vector3 s = Vector3(p) / NoiseLengthScale[i]; float d = ramp(std::fabs(obstacleDistance) / NoiseLengthScale[i]); Vector3 psi_i = BlendVectors(noise3d(s), d, gradient); psi += height_factor*NoiseGain[i]*psi_i; } Vector3 risingForce = Point3(0, 0, 0) - p; risingForce = Vector3(-risingForce[2], 0, risingForce[0]); // add rising vortex rings float ring_y = PlumeCeiling; float d = ramp(std::fabs(obstacleDistance) / RingRadius); while (ring_y > PlumeBase) { float ry = p.getY() - ring_y; float rr = std::sqrt(p.getX()*p.getX()+p.getZ()*p.getZ()); float rmag = RingMagnitude / (sqr(rr-RingRadius)+sqr(rr+RingRadius)+sqr(ry)+RingFalloff); Vector3 rpsi = rmag * risingForce; psi += BlendVectors(rpsi, d, gradient); ring_y -= RingSpeed / RingsPerSecond; } return psi; }
bool lexic(Point3 P1, Point3 P2) { return P1.getX() < P2.getX() || P1.getX() == P2.getX() && P1.getY() < P2.getY() || P1.getX() == P2.getX() && P1.getY() == P2.getY() && P1.getZ() < P2.getZ(); }
Vector3 Vector3::operator^ ( Point3 v ){ Vector3 temp; temp.setVector3( y*v.getZ() - z*v.getY(), z*v.getX() - x*v.getZ(), x*v.getY() - y*v.getX() ); return temp; }
Point3 Point3::operator-(const Point3 &other) const { Point3 cc(getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ()); return cc; }
void SetUniform(const char* name, Point3 value) { GLuint program; glGetIntegerv(GL_CURRENT_PROGRAM, (GLint*) &program); GLint location = glGetUniformLocation(program, name); glUniform3f(location, value.getX(), value.getY(), value.getZ()); }
Point3& Point3::operator*=(const Matrix3x3 &m) { Point3 cc = (*this) * m; m_x = cc.getX(); m_y = cc.getY(); m_z = cc.getZ(); return (*this); }
static float SampleDistance(Point3 p) { if (!obstacle) return (0* p.getX() + 1 * p.getY() + 0 * p.getZ() - 0.75); float phi = p.getY(); Vector3 u = p - SphereCenter; float d = length(u); return d - SphereRadius; }
void OpenGLGrabber::renderNextImageChaseCarSensors(bool renderSensors) { Position assignedNode = m_egoState; Point3 positionCamera; Point3 lookAtPointCamera; Point3 dirCamera(-10, 0, 0); dirCamera.rotateZ(assignedNode.getRotation().getAngleXY()); positionCamera.setX(assignedNode.getPosition().getX() + dirCamera.getX()); positionCamera.setY(assignedNode.getPosition().getY() + dirCamera.getY()); positionCamera.setZ(10); lookAtPointCamera.setX(assignedNode.getPosition().getX()); lookAtPointCamera.setY(assignedNode.getPosition().getY()); lookAtPointCamera.setZ(0); glPushMatrix(); glLoadIdentity(); // Setup camera. gluLookAt(positionCamera.getX(), positionCamera.getY(), positionCamera.getZ(), lookAtPointCamera.getX(), lookAtPointCamera.getY(), lookAtPointCamera.getZ(), 0, 0, 1); // Draw scene. RenderingConfiguration r = RenderingConfiguration(); m_root->render(r); // Update ego position. Point3 dir(0, 0, m_egoState.getRotation().getAngleXY()); m_car->setRotation(dir); m_car->setTranslation(m_egoState.getPosition()); m_car->render(r); if (renderSensors) { // Render sensor FOVs m_sensors->render(r); } glPopMatrix(); }
void Renderer2D::drawTriangleSet(const TriangleSet &ts, const Point3 &position, const Point3 &rotation) { vector<Point3> points; vector<Point3>::const_iterator it = ts.m_vertices.begin(); while (it != ts.m_vertices.end()) { Point3 p = (*it); p.rotateX(rotation.getX()); p.rotateY(rotation.getY()); p.rotateZ(rotation.getZ()); p += position; points.push_back(p); it++; } drawPolyLine(points); }
void Polygon::render(RenderingConfiguration &renderingConfiguration) { if ((getNodeDescriptor().getName().size() == 0) || (renderingConfiguration.getNodeRenderingConfiguration(getNodeDescriptor()).hasParameter(NodeRenderingConfiguration::ENABLED))) { glPushMatrix(); { glColor3d(m_color.getX(), m_color.getY(), m_color.getZ()); glBegin(GL_QUADS); for (uint32_t i = 0; i < m_listOfGroundVertices.size() - 1; i++) { const Point3 &p1 = m_listOfGroundVertices[i]; const Point3 &p2 = m_listOfGroundVertices[i+1]; Point3 P12 = p2 - p1; P12.setZ(0); const Point3 P1H = Point3(p1.getX(), p1.getY(), m_height); const Point3 P1HxP12 = P1H.cross(P12); glVertex3d(p1.getX(), p1.getY(), 0); glVertex3d(p1.getX(), p1.getY(), m_height); glVertex3d(p2.getX(), p2.getY(), m_height); glVertex3d(p2.getX(), p2.getY(), 0); glNormal3d(P1HxP12.getX(), P1HxP12.getY(), P1HxP12.getZ()); } glEnd(); // Bottom of the polygon. glBegin(GL_POLYGON); for (uint32_t i = 0; i < m_listOfGroundVertices.size(); i++) { const Point3 &p1 = m_listOfGroundVertices[i]; glVertex3d(p1.getX(), p1.getY(), 0); } glEnd(); // Top of the polygon. glBegin(GL_POLYGON); for (uint32_t i = 0; i < m_listOfGroundVertices.size(); i++) { const Point3 &p1 = m_listOfGroundVertices[i]; glVertex3d(p1.getX(), p1.getY(), m_height); } glEnd(); } glPopMatrix(); } }
void Camera::apply() { glLoadIdentity(); m_pitch = (m_pitch < -cartesian::Constants::PI) ? -cartesian::Constants::PI : m_pitch; m_pitch = (m_pitch > -0.01f) ? -0.01f : m_pitch; m_orientation = Point3( sin(-m_pitch) * cos(-m_head), sin(-m_pitch) * sin(-m_head), cos(-m_pitch) ); m_orientation.normalize(); m_up = Point3( sin(-m_roll) * cos(m_pitch), sin(-m_roll) * sin(m_pitch), cos(-m_roll) ); m_up.normalize(); Point3 lookAt = m_position - m_orientation; gluLookAt(m_position.getX(), m_position.getY(), m_position.getZ(), lookAt.getX(), lookAt.getY(), lookAt.getZ(), m_up.getX(), m_up.getY(), m_up.getZ()); }
int Camera::project(Point3 & pos_aos, int * viewport, float * win_coord0, float * win_coord1) { // reimplentation of gluProject (works well with perspective projection, // may not work with orthographic projection) Vectormath::Aos::Vector4 pos = Vectormath::Aos::Vector4(pos_aos.getX(), pos_aos.getY(), pos_aos.getZ(), 1.0f); Vectormath::Aos::Vector4 tmp0 = m_view_aos*pos; Vectormath::Aos::Vector4 tmp1 = m_projection_aos.getCol0()*tmp0.getX() + m_projection_aos.getCol1()*tmp0.getY() + m_projection_aos.getCol2()*tmp0.getZ() + m_projection_aos.getCol3()*tmp0.getW(); if (tmp0.getZ() == 0.0f) return 0; float tmp3 = -1.0f/tmp0.getZ(); tmp1[0] *= tmp3; tmp1[1] *= tmp3; // !! normalized *win_coord0 = ((tmp1.getX()*0.5f+0.5f)*viewport[2]+viewport[0])/viewport[2]; *win_coord1 = ((tmp1.getY()*0.5f+0.5f)*viewport[3]+viewport[1])/viewport[3]; // // wind_coord2 (z) // // This is only correct when glDepthRange(0.0, 1.0) // tmp1[2] *= tmp3; // *win_coord2=(1.0f+tmp1[2])*0.5f; return 1; }
Point3 Polygon::getCenter() const { Point3 center; if (getSize() > 0) { Point3 smallest = (*m_listOfVertices.begin()); Point3 greatest = (*m_listOfVertices.begin()); for(uint32_t i = 0; i < getSize(); i++) { Point3 point = m_listOfVertices.at(i); // Compute smallest coordinates. if (point.getX() < smallest.getX()) { smallest.setX(point.getX()); } if (point.getY() < smallest.getY()) { smallest.setY(point.getY()); } if (point.getZ() < smallest.getZ()) { smallest.setZ(point.getZ()); } // Compute greatest coordinates. if (point.getX() > greatest.getX()) { greatest.setX(point.getX()); } if (point.getY() > greatest.getY()) { greatest.setY(point.getY()); } if (point.getZ() > greatest.getZ()) { greatest.setZ(point.getZ()); } } center = (smallest + greatest) * 0.5; } return center; }
void Shader::setUniform(const char* name, Point3 value) { GLint location = glGetUniformLocation(_programInfo->glID, name); glUniform3f(location, value.getX(), value.getY(), value.getZ()); }
double Point3::getSquaredDistanceTo(const Point3 &other) const { return (other.getX() - getX())*(other.getX() - getX()) + (other.getY() - getY())*(other.getY() - getY()) + (other.getZ() - getZ())*(other.getZ() - getZ()); }
bool Point3::operator==(const Point3 &other) const { Point3 cc = (*this) - other; return ( fabs(cc.getX()) < Point3::EPSILON ) && ( fabs(cc.getY()) < Point3::EPSILON ) && ( fabs(cc.getZ()) < Point3::EPSILON ); }
float Vector3::operator* ( Point3 v ){ return ( this->x*v.getX()+this->y*v.getY()+this->z*v.getZ() ); }
Vector3 Vector3::operator-= ( Point3 b ) { this->setVector3( x - b.getX() , y - b.getY() , z - b.getZ() ); return (*this); }
Vector3 Vector3::operator+= ( Point3 b ) { this->setVector3( x + b.getX() , y + b.getY() , z + b.getZ() ); return (*this); }
double Point3::operator*(const Point3 &other) const { return getX()*other.getX() + getY()*other.getY() + getZ()*other.getZ(); }
Vector3 Vector3::operator= ( Point3 b ){ this->setVector3(b.getX(), b.getY(), b.getZ()); return *this; }
Point3 Point3::cross(const Point3 &other) const { Point3 cc(getY()*other.getZ() - getZ()*other.getY(), getZ()*other.getX() - getX()*other.getZ(), getX()*other.getY() - getY()*other.getX()); return cc; }
Vector3 Vector3::operator+ ( Point3 b ) { Vector3 temp; temp.setVector3( x + b.getX() , y + b.getY() , z + b.getZ() ); return temp; }
Vector3 Vector3::operator- ( Point3 b ) { Vector3 temp; temp.setVector3( x - b.getX() , y - b.getY() , z - b.getZ() ); return temp; }
void EnvironmentViewerGLWidget::drawScene() { if (m_root != NULL) { Lock l(m_rootMutex); if (m_cameraAssignedNodeDescriptor.getName().size() > 0) { Position assignedNode = m_mapOfCurrentPositions[m_cameraAssignedNodeDescriptor]; Point3 positionCamera; Point3 lookAtPointCamera; Point3 dirCamera(-10, 0, 0); dirCamera.rotateZ(assignedNode.getRotation().getAngleXY()); positionCamera.setX(assignedNode.getPosition().getX() + dirCamera.getX()); positionCamera.setY(assignedNode.getPosition().getY() + dirCamera.getY()); positionCamera.setZ(10); lookAtPointCamera.setX(assignedNode.getPosition().getX()); lookAtPointCamera.setY(assignedNode.getPosition().getY()); lookAtPointCamera.setZ(0); glPushMatrix(); glLoadIdentity(); // Setup camera. gluLookAt(positionCamera.getX(), positionCamera.getY(), positionCamera.getZ(), lookAtPointCamera.getX(), lookAtPointCamera.getY(), lookAtPointCamera.getZ(), 0, 0, 1); // Draw scene. m_root->render(m_renderingConfiguration); glPopMatrix(); } else { m_root->render(m_renderingConfiguration); } /* { // Visualize camera using quaternions. Position assignedNode = m_mapOfCurrentPositions[m_egoStateNodeDescriptor]; Point3 positionCamera(-10, 0, 8); const double rotX = -Constants::PI/2.0; // -90° const double rotZ = assignedNode.getRotation().getAngleXY(); Quaternion qX; qX.transform(rotX, Point3(1, 0, 0)); Quaternion qZ; qZ.transform(rotZ, Point3(0, 0, 1)); Quaternion q; q = qZ * qX; positionCamera = positionCamera * qZ.transformToMatrix3x3(); positionCamera += assignedNode.getPosition(); Point3 up(0, 1, 0); up = up * qZ.transformToMatrix3x3(); Point3 lookAtPointCamera; lookAtPointCamera.setX(assignedNode.getPosition().getX()); lookAtPointCamera.setY(assignedNode.getPosition().getY()); lookAtPointCamera.setZ(0); // Draw view direction. glColor3f(0, 1, 0); glBegin(GL_LINES); glVertex3d(positionCamera.getX(), positionCamera.getY(), positionCamera.getZ()); glVertex3d(lookAtPointCamera.getX(), lookAtPointCamera.getY(), lookAtPointCamera.getZ()); glEnd(); // Draw up direction. glColor3f(0, 0, 1); glBegin(GL_LINES); glVertex3d(positionCamera.getX(), positionCamera.getY(), positionCamera.getZ()); glVertex3d(positionCamera.getX()+5*up.getX(), positionCamera.getY()+5*up.getY(), positionCamera.getZ()+5*up.getZ()); glEnd(); } } */ /* { // Visualize parallel scanner. Position assignedNode = m_mapOfCurrentPositions[m_egoStateNodeDescriptor]; Point3 positionParallelScanner(0, 0, 1.65); const double rotZ = assignedNode.getRotation().getAngleXY(); Quaternion qZ; qZ.transform(rotZ, Point3(0, 0, 1)); positionParallelScanner = positionParallelScanner * qZ.transformToMatrix3x3(); positionParallelScanner += assignedNode.getPosition(); Point3 up(0, 1, 0); up = up * qZ.transformToMatrix3x3(); Point3 lookAtPointCamera(15, 0, 0); lookAtPointCamera.rotateZ(rotZ); lookAtPointCamera += assignedNode.getPosition(); lookAtPointCamera.setZ(0); // Draw view direction. glColor3f(0, 1, 0); glBegin(GL_LINES); glVertex3d(positionParallelScanner.getX(), positionParallelScanner.getY(), positionParallelScanner.getZ()); glVertex3d(lookAtPointCamera.getX(), lookAtPointCamera.getY(), lookAtPointCamera.getZ()); glEnd(); // Draw up direction. glColor3f(0, 0, 1); glBegin(GL_LINES); glVertex3d(positionParallelScanner.getX(), positionParallelScanner.getY(), positionParallelScanner.getZ()); glVertex3d(positionParallelScanner.getX()+5*up.getX(), positionParallelScanner.getY()+5*up.getY(), positionParallelScanner.getZ()+5*up.getZ()); glEnd(); } */ } }