#include <cmath>
+#include <msp/geometry/box.h>
+#include <msp/geometry/circle.h>
+#include <msp/geometry/extrudedshape.h>
+#include <msp/geometry/halfspace.h>
+#include <msp/geometry/intersection.h>
+#include <msp/geometry/negation.h>
+#include <msp/geometry/transformedshape.h>
#include "trackpart.h"
using namespace std;
namespace R2C2 {
TrackPart::TrackPart():
- dir(0),
length(0),
radius(0),
path(0),
- dead_end(false)
+ dead_end(false),
+ shape(0)
{
links[0] = 0;
links[1] = 0;
}
+TrackPart::~TrackPart()
+{
+ delete shape;
+}
+
float TrackPart::get_length() const
{
if(radius)
return length;
}
-TrackPoint TrackPart::get_point(float d) const
+OrientedPoint TrackPart::get_point(float d) const
{
- TrackPoint result;
+ OrientedPoint result;
if(radius)
{
- float a = d/radius;
- float c = cos(a);
- float s = sin(a);
- float rx = radius*sin(dir);
- float ry = -radius*cos(dir);
- result.pos = Vector(pos.x+c*rx-s*ry-rx, pos.y+c*ry+s*rx-ry);
- result.dir = dir+a;
+ Angle a = Angle::from_radians(d/radius);
+ result.position = transform.transform(Vector(sin(a)*radius, (1-cos(a))*radius, 0));
+ result.rotation = dir+a;
}
else
{
- result.pos = Vector(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
- result.dir = dir;
+ result.position = transform.transform(Vector(d, 0, 0));
+ result.rotation = dir;
}
return result;
}
+OrientedPoint TrackPart::get_nearest_point(const Vector &p) const
+{
+ OrientedPoint result;
+ if(radius)
+ {
+ Vector v = p-transform.transform(Vector(0, radius, 0));
+ Angle a = Geometry::atan2(v.y, v.x)+Angle::quarter_turn()-dir;
+ if(radius<0)
+ a = Angle::half_turn()-a;
+ a.wrap_with_base(Angle::from_radians(length/2)-Angle::half_turn());
+ a = min(max(a, Angle::zero()), Angle::from_radians(length));
+ if(radius<0)
+ a = -a;
+ result.position = transform.transform(Vector(sin(a)*radius, (1-cos(a))*radius, 0));
+ result.rotation = dir+a;
+ }
+ else
+ {
+ Vector v = p-pos;
+ Vector dir_vec = transform.transform_linear(Vector(1, 0, 0));
+ float d = min(max(dot(dir_vec, v), 0.0f), length);
+ result.position = pos+dir_vec*d;
+ result.rotation = dir;
+ }
+ return result;
+}
+
void TrackPart::check_link(TrackPart &other)
{
unsigned n_eps = (dead_end ? 1 : 2);
unsigned n_other_eps = (other.is_dead_end() ? 1 : 2);
for(unsigned i=0; i<n_eps; ++i)
{
- TrackPoint p1 = get_point(i ? get_length() : 0);
+ OrientedPoint p1 = get_point(i ? get_length() : 0);
for(unsigned j=0; j<n_other_eps; ++j)
{
- TrackPoint p2 = other.get_point(j ? other.get_length() : 0);
+ OrientedPoint p2 = other.get_point(j ? other.get_length() : 0);
- float dx = p2.pos.x-p1.pos.x;
- float dy = p2.pos.y-p1.pos.y;
+ Vector span = p2.position-p1.position;
- float da = p2.dir-p1.dir+M_PI*((i+j+1)%2);
- while(da>M_PI)
- da -= M_PI*2;
- while(da<-M_PI)
- da += M_PI*2;
+ Angle da = wrap_balanced(p2.rotation-p1.rotation+Angle::half_turn()*float((i+j+1)%2));
- if(dx*dx+dy*dy<1e-6 && da>=-0.01 && da<=0.01)
+ if(dot(span, span)<1e-6 && abs(da).radians()<=0.01)
{
links[i] = &other;
other.links[j] = this;
return links[i];
}
-bool TrackPart::collide_ray(const Vector &start, const Vector &ray, float width) const
+void TrackPart::create_shape()
{
- Vector local_start(start.x-pos.x, start.y-pos.y, start.z);
- float c = cos(dir);
- float s = sin(dir);
- local_start = Vector(c*local_start.x+s*local_start.y, c*local_start.y-s*local_start.x, local_start.z);
- Vector local_ray(c*ray.x+s*ray.y, c*ray.y-s*ray.x, ray.z);
-
- float d = -local_start.z/local_ray.z;
- if(d<0)
- return false;
-
- Vector base(local_start.x+d*local_ray.x, local_start.y+d*local_ray.y);
-
if(radius)
{
- base.y -= radius;
- if(radius<0)
- base.y = -base.y;
- float r = sqrt(base.x*base.x+base.y*base.y)-abs(radius);
- float a = atan2(base.x, -base.y);
- return (a>=0 && a<=length && r>=-width/2 && r<=width/2);
+ Geometry::ExtrudedShape<float, 3> ring(
+ Geometry::Intersection<float, 2>(
+ Geometry::Circle<float>(abs(radius)+0.02),
+ Geometry::Negation<float, 2>(Geometry::Circle<float>(abs(radius)-0.02))),
+ 0.01);
+ Geometry::HalfSpace<float, 3> start_wall(Vector(-1, 0, 0));
+ Geometry::HalfSpace<float, 3> end_wall(rotated_vector(Vector(1, 0, 0),
+ Angle::from_radians(length*(radius<0 ? -1 : 1))));
+ /* This box only exists so the bounding boxes of curves make sense. It
+ can be removed when libmspmath produces better bounding boxes. */
+ float w = sin(length)*(abs(radius)+0.02);
+ float h = (1-cos(length))*abs(radius-0.02)+0.04;
+ Geometry::TransformedShape<float, 3> bounds(
+ Geometry::Box<float>(w, h, 0.01),
+ Transform::translation(Vector(w/2, (h/2-0.02)*(radius<0 ? -1 : 1)-radius, 0.005)));
+ vector<const Shape *> shapes;
+ shapes.push_back(&ring);
+ shapes.push_back(&start_wall);
+ shapes.push_back(&end_wall);
+ shapes.push_back(&bounds);
+ shape = new Geometry::TransformedShape<float, 3>(
+ Geometry::Intersection<float, 3>::from_iterator_range(shapes.begin(), shapes.end()),
+ transform*Transform::translation(Vector(0, radius, 0.005)));
}
else
- return (base.x>=0 && base.x<=length && base.y>=-width/2 && base.y<=width/2);
+ {
+ // TODO Get the track profile dimensions from somewhere
+ shape = new Geometry::TransformedShape<float, 3>(
+ Geometry::Box<float>(length, 0.04, 0.01),
+ transform*Transform::translation(Vector(length/2, 0, 0.005)));
+ }
}
else
obj.length /= 1000;
- obj.pos.x /= 1000;
- obj.pos.y /= 1000;
- obj.dir *= M_PI/180;
+ obj.pos /= 1000;
+
+ obj.transform = Transform::translation(obj.pos)*
+ Transform::rotation(obj.dir, Vector(0, 0, 1));
}
void TrackPart::Loader::start(float x, float y, float d)
{
- obj.pos = Vector(x, y);
- obj.dir = d;
+ obj.pos = Vector(x, y, 0);
+ obj.dir = Angle::from_degrees(d);
}
} // namespace R2C2
projects / r2c2.git / blobdiff