Hemic driver added

src/drivers/hemic/Makefile

@@ -0,0 +1,29 @@
+##############################################################################
+#
+#    file                 : Makefile
+#    created              : Mo 8. Jan 00:39:15 CET 2018
+#    copyright            : (C) 2002 Jonas Natzer, Michael Heinrich
+#
+##############################################################################
+#
+#   This program is free software; you can redistribute it and/or modify
+#   it under the terms of the GNU General Public License as published by
+#   the Free Software Foundation; either version 2 of the License, or
+#   (at your option) any later version.
+#
+##############################################################################
+
+ROBOT       = hemic
+MODULE      = ${ROBOT}.so
+MODULEDIR   = drivers/${ROBOT}
+SOURCES     = ${ROBOT}.cpp TrackData.cpp Trajectory.cpp optimal_line.cpp
+
+SHIPDIR     = drivers/${ROBOT}
+SHIP        = ${ROBOT}.xml 155-DTM.rgb logo.rgb
+SHIPSUBDIRS = 
+
+PKGSUBDIRS  = ${SHIPSUBDIRS}
+src-robots-hemic_PKGFILES = $(shell find * -maxdepth 0 -type f -print)
+src-robots-hemic_PKGDIR   = ${PACKAGE}-${VERSION}/$(subst ${TORCS_BASE},,$(shell pwd))
+
+include ${MAKE_DEFAULT}

src/drivers/hemic/TrackData.cpp

@@ -0,0 +1,103 @@
+// -*- Mode: c++ -*-
+// copyright (c) 2006 by Christos Dimitrakakis <[email protected]>
+/***************************************************************************
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ ***************************************************************************/
+#include "TrackData.h"
+#include <tgf.h>
+
+TrackData::TrackData()
+{
+    mid.x = mid.y = mid.z = 0.0f;
+    width_l = width_r = 10.0;
+    angle = 0.0;
+    step = 5.0;
+}
+
+void TrackData::setWidth(float width)
+{
+    width_l = width_r = width / 2.0f;
+    assert(width_r > -width_l);
+}
+
+void TrackData::setLeftWidth(float width)
+{
+    width_l = width;
+    assert(width_r > -width_l);
+}
+
+void TrackData::setRightWidth(float width)
+{
+    width_r = width;
+    assert(width_r > -width_l);
+}
+
+void TrackData::setStep(float step)
+{
+    assert(step > 0.0f);
+    this->step = step;
+}
+
+void TrackData::AddStraight(SegmentList& segments, float length, float end_width_l, float end_width_r)
+{
+    int N = 1 + (int) floor(length / step);
+    float s = length / (float) N;
+    float d_width_l = (end_width_l - width_l) / (float) N;
+    float d_width_r = (end_width_r - width_r) / (float) N;
+    float hpi = PI / 2.0f;
+    for (int i = 0; i < N; ++i)
+    {
+        mid.x += s * sin(angle);
+        mid.y += s * cos(angle);
+        Point left(mid.x + width_l * sin(angle - hpi),
+                   mid.y + width_l * cos(angle - hpi),
+                   mid.z);
+        Point right(mid.x + width_r * sin(angle + hpi),
+                    mid.y + width_r * cos(angle + hpi),
+                    mid.z);
+        segments.Add(Segment(left, right));
+        width_l += d_width_l;
+        width_r += d_width_r;
+    }
+
+    width_l = end_width_l;
+    width_r = end_width_r;
+}
+
+/// arc in radians
+
+void TrackData::AddCurve(SegmentList& segments, float arc, float radius, float end_width_l, float end_width_r)
+{
+    arc = arc * PI / 180.0f;
+    float length = fabs(arc) * radius;
+    int N = 1 + (int) floor(length / step);
+    float s = length / (float) N;
+    float d_width_l = (end_width_l - width_l) / (float) N;
+    float d_width_r = (end_width_r - width_r) / (float) N;
+    float d_angle = arc / (float) N;
+    float start_angle = angle;
+    float hpi = (float) (PI / 2.0);
+    for (int i = 0; i < N; ++i)
+    {
+        mid.x += s * sin(angle);
+        mid.y += s * cos(angle);
+        Point left(mid.x + width_l * sin(angle - hpi),
+                   mid.y + width_l * cos(angle - hpi),
+                   mid.z);
+        Point right(mid.x + width_r * sin(angle + hpi),
+                    mid.y + width_r * cos(angle + hpi),
+                    mid.z);
+        segments.Add(Segment(left, right));
+        angle += d_angle;
+        width_l += d_width_l;
+        width_r += d_width_r;
+    }
+    width_l = end_width_l;
+    width_r = end_width_r;
+    angle = start_angle + arc;
+}

src/drivers/hemic/TrackData.h

@@ -0,0 +1,203 @@
+// -*- Mode: c++ -*-
+// copyright (c) 2006 by Christos Dimitrakakis <[email protected]>
+/***************************************************************************
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifndef TRACKDATA_H
+#define TRACKDATA_H
+
+#include <cstdio>
+#include <cstdlib>
+#include <cmath>
+#include <cassert>
+#include <list>
+#include <vector>
+
+class Point
+{
+protected:
+    float _length;
+public:
+    float x;
+    float y;
+    float z;
+
+    Point()
+    {
+        _length = -1.0f;
+    }
+
+    Point(float x, float y, float z = 0.0)
+    {
+        this->x = x;
+        this->y = y;
+        this->z = z;
+        _length = -1.0f;
+    }
+
+    float Length()
+    {
+        if (_length < 0)
+        {
+            _length = sqrt(x * x + y * y + z * z);
+        }
+        return _length;
+    }
+
+    void Normalise()
+    {
+        float s = 1.0f / Length();
+        x *= s;
+        y *= s;
+        z *= s;
+        _length = 1.0f;
+    }
+
+    Point& operator=(const Point& rhs)
+    {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        return *this;
+    }
+
+    Point& operator-=(const Point& rhs)
+    {
+        x -= rhs.x;
+        y -= rhs.y;
+        z -= rhs.z;
+        return *this;
+    }
+
+    Point& operator+=(const Point& rhs)
+    {
+        x += rhs.x;
+        y += rhs.y;
+        z += rhs.z;
+        return *this;
+    }
+
+    Point operator+(const Point& rhs)
+    {
+        Point lhs;
+        lhs.x = x + rhs.x;
+        lhs.y = y + rhs.y;
+        lhs.z = z + rhs.z;
+        return lhs;
+    }
+
+    Point operator-(const Point& rhs)
+    {
+        Point lhs;
+        lhs.x = x - rhs.x;
+        lhs.y = y - rhs.y;
+        lhs.z = z - rhs.z;
+        return lhs;
+    }
+
+    Point& operator*=(const float& rhs)
+    {
+        x *= rhs;
+        y *= rhs;
+        z *= rhs;
+        return *this;
+    }
+
+    Point operator*(const float& rhs)
+    {
+        Point lhs;
+        lhs.x = x*rhs;
+        lhs.y = y*rhs;
+        lhs.z = z*rhs;
+        return lhs;
+    }
+
+    Point& operator/=(const float& rhs)
+    {
+        x /= rhs;
+        y /= rhs;
+        z /= rhs;
+        return *this;
+    }
+
+    Point operator/(const float& rhs)
+    {
+        Point lhs;
+        lhs.x = x / rhs;
+        lhs.y = y / rhs;
+        lhs.z = z / rhs;
+        return lhs;
+    }
+
+};
+
+class Segment
+{
+public:
+    Point left;
+    Point right;
+
+    Segment(Point left, Point right)
+    {
+        this->left = left;
+        this->right = right;
+    }
+};
+
+class SegmentList
+{
+protected:
+    std::vector<Segment> segments;
+public:
+
+    void Add(Segment segment)
+    {
+        segments.push_back(segment);
+    }
+
+    Segment& operator[](int i)
+    {
+        return segments[i];
+    }
+
+    int size()
+    {
+        return segments.size();
+    }
+
+    void PrintSegments()
+    {
+        int N = segments.size();
+        for (int i = 0; i < N; ++i)
+        {
+            printf("%f %f %f %f\n",
+                   segments[i].left.x, segments[i].left.y,
+                   segments[i].right.x, segments[i].right.y);
+        }
+    }
+};
+
+class TrackData
+{
+    float width_l;
+    float width_r;
+    float angle;
+    float step;
+    Point mid;
+public:
+    TrackData();
+    void setWidth(float width);
+    void setLeftWidth(float width);
+    void setRightWidth(float width);
+    void setStep(float step);
+    void AddStraight(SegmentList& segments, float length, float end_width_l, float end_width_r);
+    void AddCurve(SegmentList& segments, float arc, float radius, float end_width_l, float end_width_r);
+};
+
+#endif