namespace R2C2 {
TrackPart::TrackPart():
- dir(0),
length(0),
radius(0),
path(0),
{
TrackPoint result;
+ Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
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);
+ Angle a = Angle::from_radians(d/radius);
+ Vector r = dir_trans.transform(Vector(0, -radius, 0));
+ result.pos = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
result.dir = dir+a;
}
else
{
- result.pos = Vector(pos.x+cos(dir)*d, pos.y+sin(dir)*d);
+ result.pos = pos+dir_trans.transform(Vector(d, 0, 0));
result.dir = dir;
}
TrackPoint TrackPart::get_nearest_point(const Vector &p) const
{
TrackPoint tp;
- float c = cos(dir);
- float s = sin(dir);
+ Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
if(radius)
{
- Vector v(p.x-pos.x+radius*s, p.y-pos.y-radius*c);
- float a = atan2(v.y, v.x)+M_PI/2-dir;
+ Vector r = dir_trans.transform(Vector(0, -radius, 0));
+ Vector v = p-pos+r;
+ Angle a = Geometry::atan2(v.y, v.x)+Angle::quarter_turn()-dir;
if(radius<0)
- a = M_PI-a;
- while(a<length/2-M_PI)
- a += M_PI*2;
- while(a>length/2+M_PI)
- a -= M_PI*2;
- a = min(max(a, 0.0f), length);
+ 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;
- tp.pos = Vector(pos.x+radius*(sin(dir+a)-s), pos.y+radius*(c-cos(dir+a)));
+ tp.pos = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
tp.dir = dir+a;
}
else
{
- Vector v(p.x-pos.x, p.y-pos.y);
- float d = min(max(c*v.x+s*v.y, 0.0f), length);
- tp.pos = Vector(pos.x+c*d, pos.y+s*d);
+ Vector v = p-pos;
+ Vector dir_vec = dir_trans.transform(Vector(1, 0, 0));
+ float d = min(max(dot(dir_vec, v), 0.0f), length);
+ tp.pos = pos+dir_vec*d;
tp.dir = dir;
}
return tp;
{
TrackPoint 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.pos-p1.pos;
- 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.dir-p1.dir+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;
bool TrackPart::collide_ray(const Vector &start, const Vector &ray, float width) const
{
- 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);
+ Transform trans = Transform::rotation(dir, Vector(0, 0, -1));
+ Vector local_start = trans.transform(start);
+ Vector local_ray = trans.transform_linear(ray);
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);
+ Vector base = local_start+d*local_ray;
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 r = base.norm()-abs(radius);
float a = atan2(base.x, -base.y);
return (a>=0 && a<=length && r>=-width/2 && r<=width/2);
}
else
obj.length /= 1000;
- obj.pos.x /= 1000;
- obj.pos.y /= 1000;
- obj.dir *= M_PI/180;
+ obj.pos /= 1000;
}
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