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();
}
void PointSensor::drawFOV(QPainter &painter) const {
Point3 A = m_translation;
double totalRot = m_rotZ * cartesian::Constants::DEG2RAD;
Point3 B(m_distanceFOV, 0, 0);
B.rotateZ(totalRot + (m_angleFOV/2.0) * cartesian::Constants::DEG2RAD);
B += m_translation;
Point3 C(m_distanceFOV, 0, 0);
C.rotateZ(totalRot - (m_angleFOV/2.0) * cartesian::Constants::DEG2RAD);
C += m_translation;
painter.drawLine(A.getX() * 1000, A.getY() * 1000, B.getX() * 1000, B.getY() * 1000);
painter.drawLine(A.getX() * 1000, A.getY() * 1000, C.getX() * 1000, C.getY() * 1000);
double stepSize = 2.0; // 2° to rad.
uint32_t steps = (unsigned int) round( m_angleFOV/stepSize );
Point3 old = C;
for(uint32_t i = 0; i < steps; i++) {
// Calculate the skeleton approximation.
Point3 p;
p.setX(m_distanceFOV * cos(totalRot - (m_angleFOV/2.0) * cartesian::Constants::DEG2RAD + i * stepSize * cartesian::Constants::DEG2RAD));
p.setY(m_distanceFOV * sin(totalRot - (m_angleFOV/2.0) * cartesian::Constants::DEG2RAD + i * stepSize * cartesian::Constants::DEG2RAD));
p += m_translation;
painter.drawLine(old.getX() * 1000, old.getY() * 1000, p.getX() * 1000, p.getY() * 1000);
old = p;
}
painter.drawLine(old.getX() * 1000, old.getY() * 1000, B.getX() * 1000, B.getY() * 1000);
}
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;
}
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;
}
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 Point3 &other) const {
Point3 cc(getX() - other.getX(),
getY() - other.getY(),
getZ() - other.getZ());
return cc;
}
Point3& Point3::operator*=(const Matrix3x3 &m) {
Point3 cc = (*this) * m;
m_x = cc.getX();
m_y = cc.getY();
m_z = cc.getZ();
return (*this);
}
const WGS84Coordinate WGS84Coordinate::transform(const Point3 &coordinate, const double &accuracy) const {
WGS84Coordinate result(*this);
Point3 point3Result;
double addLon = 1e-7;
int32_t signLon = (coordinate.getX() < 0) ? -1 : 1;
double addLat = 1e-7;
int32_t signLat = (coordinate.getY() < 0) ? -1 : 1;
double epsilon = accuracy;
if (epsilon < 0) {
epsilon = 1e-2;
}
point3Result = transform(result);
double dOld = numeric_limits<double>::max();
double d = abs(coordinate.getY() - point3Result.getY());
uint32_t iterations = 0;
// while ( (d < dOld) && (d > epsilon) && (iterations < 50000)) {
while ( (d < dOld) && (d > epsilon) ) {
result = WGS84Coordinate(result.getLatitude() + signLat * addLat, result.getLATITUDE(), result.getLongitude(), result.getLONGITUDE());
point3Result = transform(result);
dOld = d;
d = abs(coordinate.getY() - point3Result.getY());
iterations++;
}
// Use the last transformed point3Result here.
dOld = numeric_limits<double>::max();
d = abs(coordinate.getX() - point3Result.getX());
iterations = 0;
// while ( (d < dOld) && (d > epsilon) && (iterations < 50000)) {
while ( (d < dOld) && (d > epsilon) ) {
result = WGS84Coordinate(result.getLatitude(), result.getLATITUDE(), result.getLongitude() + signLon * addLon, result.getLONGITUDE());
point3Result = transform(result);
dOld = d;
d = abs(coordinate.getX() - point3Result.getX());
iterations++;
}
return result;
}
void HeightGrid::init() {
if (m_heightImage != NULL) {
clog << "Generate terrain data: " << m_heightImage->getWidth() << ", " << m_heightImage->getHeight() << ", Format: " << m_heightImage->getFormat() << ", width step: " << m_heightImage->getWidthStep() << endl;
MyImage<BGRPixel> img(m_heightImage);
// TODO: Compute normals.
float scaleZ = m_max - m_min;
if (scaleZ < 0) {
scaleZ = 1.0f;
}
if ( (m_ground < 0) || (m_ground > 1) ) {
m_ground = 0.5;
}
clog << "Ground height: " << m_ground << ", scaling Z : " << scaleZ << ", translation for z-direction: " << (-1 * scaleZ * m_ground) << endl;
m_callList = glGenLists(1);
glNewList(m_callList, GL_COMPILE);
for (uint32_t y = 0; y < m_heightImage->getHeight() - 2; y++) {
glBegin(GL_TRIANGLE_STRIP);
for (uint32_t x = 0; x < m_heightImage->getWidth(); x++) {
glColor3f(static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f, static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f, static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f);
glVertex3f(static_cast<float>(x), static_cast<float>(y), (static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - y)->r) / 255.0f - m_ground) * scaleZ);
glVertex3f(static_cast<float>(x), static_cast<float>(y + 1), (static_cast<int>(img.getPixel(x, m_heightImage->getHeight() - 1 - (y + 1))->r) / 255.0f - m_ground) * scaleZ);
}
glEnd();
}
glEndList();
// Compute translation.
Point3 translate;
translate.setX(-1 * m_originPixelXY.getX() * m_scalingPixelXY.getX());
translate.setY(-1 * (m_heightImage->getHeight() - m_originPixelXY.getY()) * m_scalingPixelXY.getY());
Point3 scale(m_scalingPixelXY);
// m_scalingPixelXY sets z to 0, thus elevation is disable. Therefore, scale z to 1.0, i.e. keep computed elevation.
scale.setZ(1.0);
// Set up the actual renderer.
m_heightImageRenderer = new HeightGridRenderer(getNodeDescriptor(), m_callList);
// Set up transform group.
m_heightImageNode = new TransformGroup();
m_heightImageNode->addChild(m_heightImageRenderer);
// Translate the height image.
m_heightImageNode->setTranslation(Point3(translate.getX(), translate.getY(), 0));
// Scale the height image.
m_heightImageNode->setScaling(scale);
}
}
void CheckerBoard::render(const RenderingConfiguration &renderingConfiguration) const {
if ((getNodeDescriptor().getName().size() == 0) || (renderingConfiguration.getNodeRenderingConfiguration(getNodeDescriptor()).hasParameter(NodeRenderingConfiguration::ENABLED))) {
glPushMatrix();
{
bool color = false;
uint32_t squares = 0;
double height = 0;
while (height < m_height) {
Point3 p = m_positionA;
double d = p.getDistanceTo(m_positionB);
squares = 0;
while (d > 0.1) {
if (color) {
glColor3d(1, 1, 1);
} else {
glColor3d(0.1, 0.1, 0.1);
}
glBegin(GL_QUADS);
{
glVertex3d(p.getX(), p.getY(), height);
glVertex3d(p.getX(), p.getY(), height + 0.1);
p += Point3(0.1, 0, 0);
glVertex3d(p.getX(), p.getY(), height + 0.1);
glVertex3d(p.getX(), p.getY(), height);
}
glEnd();
color = !color;
d = p.getDistanceTo(m_positionB);
squares++;
}
if ((squares % 2) == 0) {
color = !color;
}
height += 0.1;
}
}
glPopMatrix();
}
}
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 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();
}
*/
}
}
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;
}
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());
}
double Point3::operator*(const Point3 &other) const {
return getX()*other.getX() + getY()*other.getY() + getZ()*other.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(b.getX(), b.getY(), b.getZ());
return *this;
}
Vector3 Vector3::operator+ ( Point3 b )
{
Vector3 temp;
temp.setVector3( x + b.getX() , y + b.getY() , z + b.getZ() );
return temp;
}
void Shader::setUniform(const char* name, Point3 value)
{
GLint location = glGetUniformLocation(_programInfo->glID, name);
glUniform3f(location, value.getX(), value.getY(), value.getZ());
}
Vector3 Vector3::operator+= ( Point3 b )
{
this->setVector3( x + b.getX() , y + b.getY() , z + b.getZ() );
return (*this);
}
Vector3 Vector3::operator- ( Point3 b )
{
Vector3 temp;
temp.setVector3( x - b.getX() , y - b.getY() , z - b.getZ() );
return temp;
}
