X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Ftrackpart.cpp;h=d8dac075e68701dfc45d8dc3204d315335dd1d5b;hb=7a36d396eded897c421424905b2c938d770df341;hp=79e183b5a9ce54ad3892f488270882bce4511e7f;hpb=d15ac13f2e170f155b4bbd124df48400c339b644;p=r2c2.git

diff --git a/source/libr2c2/trackpart.cpp b/source/libr2c2/trackpart.cpp
index 79e183b..d8dac07 100644
--- a/source/libr2c2/trackpart.cpp
+++ b/source/libr2c2/trackpart.cpp
@@ -7,7 +7,6 @@ using namespace Msp;
 namespace R2C2 {
 
 TrackPart::TrackPart():
-	dir(0),
 	length(0),
 	radius(0),
 	path(0),
@@ -29,25 +28,52 @@ TrackPoint TrackPart::get_point(float d) const
 {
 	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;
 	}
 
 	return result;
 }
 
+TrackPoint TrackPart::get_nearest_point(const Vector &p) const
+{
+	TrackPoint tp;
+	Transform dir_trans = Transform::rotation(dir, Vector(0, 0, 1));
+	if(radius)
+	{
+		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 = 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 = pos-r+Transform::rotation(a, Vector(0, 0, 1)).transform(r);
+		tp.dir = dir+a;
+	}
+	else
+	{
+		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;
+}
+
 void TrackPart::check_link(TrackPart &other)
 {
 	unsigned n_eps = (dead_end ? 1 : 2);
@@ -59,16 +85,11 @@ void TrackPart::check_link(TrackPart &other)
 		{
 			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;
@@ -81,30 +102,28 @@ void TrackPart::check_link(TrackPart &other)
 TrackPart *TrackPart::get_link(unsigned i) const
 {
 	if(i>=2)
-		throw InvalidParameterValue("Index out of range");
+		throw out_of_range("TrackPart::get_link");
 	return links[i];
 }
 
 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);
 	}
@@ -114,7 +133,7 @@ bool TrackPart::collide_ray(const Vector &start, const Vector &ray, float width)
 
 
 TrackPart::Loader::Loader(TrackPart &p):
-	Msp::DataFile::BasicLoader<TrackPart>(p)
+	Msp::DataFile::ObjectLoader<TrackPart>(p)
 {
 	add("start",    &Loader::start);
 	add("length",   &TrackPart::length);
@@ -133,15 +152,13 @@ void TrackPart::Loader::finish()
 	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