geometry.cpp

2002年

Line Rect::BottomEdge() const
{
	Line l;
	l.LineFromTwoPoints(BottomLeftCorner(), BottomRightCorner());
	return l;
}

Line Rect::nearestHEdgeLine(const Vector& p) const
{
  return (fabs((p.y - top_y)) < fabs((bottom_y - p.y))) ? TopEdge() : BottomEdge() ;
}

Line Rect::nearestVEdgeLine(const Vector& p) const
{
  return (fabs((p.x - left_x)) < fabs((right_x - p.x))) ? LeftEdge() : RightEdge() ;
}

Line Rect::nearestEdgeLine(const Vector& p) const
{
  if(Min((float)fabs((p.x-left_x)),(float)fabs((right_x-p.x))) < Min((float)fabs((p.y-top_y)),(float)fabs((bottom_y-p.y))))
    return nearestVEdgeLine(p) ;
  else
    return nearestHEdgeLine(p) ;  
}

bool Rect::RayIntersection(const Ray& r,Vector &p) const
{
	Vector crosspoint;
	Vector returnp;
	float crossdist = -1;
	if (r.intersection(LeftEdge(),crosspoint))
	{
		if (crosspoint.y >=top_y && crosspoint.y <= bottom_y && (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (r.intersection(RightEdge(),crosspoint))
	{
		if (crosspoint.y >=top_y && crosspoint.y <=bottom_y && (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (r.intersection(TopEdge(),crosspoint))
	{
		if (crosspoint.x >= left_x && crosspoint.x <= right_x && (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (r.intersection(BottomEdge(),crosspoint))
	{
		if (crosspoint.x >=left_x && crosspoint.x <= right_x&& (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (crossdist  != -1)
	{
		p = returnp;
		return true;
	}
	else
	{
		p = Vector(0,0);
		return false;
	}
}


/*** The3rdCurve ********/
The3rdCurve::The3rdCurve(float a, float b, float c, float d, float inf, float sup){
	SetABCD(a, b, c, d);
	SetInfSup(inf, sup);
}

void The3rdCurve::SetABCD(float a, float b, float c, float d){
	this->a = a;
	this->b = b;
	this->c = c;
	this->d = d;
}

void The3rdCurve::SetInfSup(float inf, float sup){
	this->inf = inf;
	this->sup = sup;
}

float The3rdCurve::GetOutput(float x, bool limited) const
{
	float result = a *x * x *x + b * x * x + c * x + d;
	if (limited){
		pose_limitation(result, inf, sup);
	}
	return result;
}

bool The3rdCurve::Interpolation(float x0, float y0, float x1, float y1, float x2, float y2, float x3, float y3){
	if (x0 == x1 || x0 == x2 || x0 == x3 || x1 == x2 || x1 == x3 || x2 == x3) return false;
	float t0, t1, t2, t3;
	t0 = y0 / ((x0 - x1) * (x0 - x2) * (x0 - x3));
	t1 = y1 / ((x1 - x0) * (x1 - x2) * (x1 - x3));
	t2 = y2 / ((x2 - x0) * (x2 - x1) * (x2 - x3));
	t3 = y3 / ((x3 - x0) * (x3 - x1) * (x3 - x2));

	a = t0 + t1 + t2 + t3;
	b = - (t0 * (x1 + x2 + x3) + t1 * (x0 + x2 + x3) + t2 * (x0 + x1 + x3) + t3 * (x0 + x1 +x2));
	c = t0 * (x1 * x2 + x1 * x3 + x2 * x3) + t1 * (x0 * x2 + x0 * x3 + x2 * x3)
		+ t2 * (x1 * x0 + x3 * x1 + x3 * x0) + t3 * (x0 * x1 + x0 * x2 + x1 * x2);
	d = -(t0 * (x1 * x2 * x3) + t1 * (x0 * x2 * x3) + t2 * (x0 * x1 * x3) + t3 * (x0 * x1 * x2));
	return true;
}
/************************Ring Sector****************************/
//to determine whether a specific angle is in PossSpace angle section
bool RingSector::IsWithin(float angle){
	if(!valid) return false;

	int i;
	for(i=0; i<angles.Num_datas(); i++){
		if( AngleDifference(angles.Data(i).mean, angle) < angles.Data(i).delta){
			return true;
		}
	}
	return false;
}

///space became larger as time goes by
void RingSector::Diffuse(int time, float speed){
	if(!valid) return;

	radius.delta = Min(radius.mean , radius.delta + float(time*speed));

	int i;
	for(i=0; i<angles.Num_datas(); i++){
		angles.Data(i).delta += Rad2Deg(time*speed/radius.mean);
	}
	//check whether to merge
	for(i=angles.Num_datas()-1 ; i>0; i--){
		float dif = AngleDifference(angles.Data(i).mean, angles.Data(0).mean);
		if( dif < angles.Data(i).delta + angles.Data(0).delta){
			if(NormalizeAngle(angles.Data(i).mean - angles.Data(0).mean) > 0){
				angles.Data(0).mean = NormalizeAngle(angles.Data(0).mean +
					(angles.Data(i).delta - angles.Data(0).delta + dif)/2) ; 
			}else{
				angles.Data(0).mean = NormalizeAngle(angles.Data(i).mean +
					(angles.Data(0).delta - angles.Data(i).delta + dif)/2) ; 
			}
			angles.Data(0).delta += dif/2;
			angles.RmTail();
			continue;
		}
		break;
	}
}

void RingSector::MinusRadius(float inf, float sup){
	if(!valid) return;
	float n_sup = Min(radius.sup(), sup);
	float n_inf = Max(radius.inf(), inf);
	if(n_inf > n_sup){
		valid = false;
		return;
	}
	radius.mean = (n_sup + n_inf) / 2;
	radius.delta = (n_sup - n_inf) / 2;

}
//sight infor may diminish the RingSector
//Problem: when radius is too far and can't be seen, what it mean?
void RingSector::MinusAngle(NoiseData& subtrahend){
	if(!valid) return;

	int i;
	float default_angle = angles.Data(0).mean;
	for(i=angles.Num_datas()-1; i>=0; i--){
		float dif = AngleDifference(angles.Data(i).mean, subtrahend.mean);
		if(dif >= angles.Data(i).delta + subtrahend.delta) continue;
		if(dif + angles.Data(i).delta <= subtrahend.delta){
			if(angles.Num_datas()-1 > i){
				angles.Data(i) = angles.Data(angles.Num_datas()-1);
			}
			angles.RmTail();
			continue;
		}
		if( dif+subtrahend.delta <= angles.Data(i).delta){
			angles.Add(CreateAngleSector(
						subtrahend.mean + subtrahend.delta,
						angles.Data(i).mean + angles.Data(i).delta
						));
			angles.Data(i) = CreateAngleSector(
						angles.Data(i).mean - angles.Data(i).delta,
						subtrahend.mean - subtrahend.delta
						);
			continue;
		}
		if(NormalizeAngle(subtrahend.mean - angles.Data(i).mean) > 0){
			angles.Data(i) = CreateAngleSector(
				angles.Data(i).mean - angles.Data(i).delta,
				subtrahend.mean - subtrahend.delta
				);
		}else{
			angles.Data(i) = CreateAngleSector(
				subtrahend.mean + subtrahend.delta,
				angles.Data(i).mean + angles.Data(i).delta
			);
		}
	}

	if(angles.Num_datas() == 0){
		valid = false;
	}else if(angles.Num_datas() > 1){		
		if(angles.Data(0).delta < angles.Data(1).delta){
			Swap(angles.Data(0), angles.Data(1));
		}
	}
}

void RingSector::Intersect(NoiseData r, NoiseData a){
	if(!valid) return;

	//intersect radius range
	float dif_r = (float)fabs(r.mean - radius.mean);
	if(dif_r >= r.delta + radius.delta ){
		angles.Reset();
		valid = false;
		return;
	}
	radius.Intersect(r);
	float f;
	int i;
	for(i=angles.Num_datas()-1; i>=0; i--){
		f=a.mean - angles.Data(i).mean;
		if(f < -180.0f) a.mean += 360.0f;
		else if (f > 180.0f) angles.Data(i).mean += 360.0f;
		if(!angles.Data(i).Intersect(a)){
			angles.Data(i) = angles.Data(angles.Num_datas()-1);
			angles.RmTail();
		}else{
			angles.Data(i).mean = NormalizeAngle(angles.Data(i).mean);
		}
	}
	if(angles.Num_datas() == 0){
		valid = false;
	}else if(angles.Num_datas() > 1){
		if(angles.Data(0).delta < angles.Data(1).delta){
			Swap(angles.Data(0), angles.Data(1));
		}
	} 
}

//Note: only the main angle range is consideded
void RingSector::ShiftCenter(const Vector& delta){
	Vector v =  Polar2Vector(radius.mean, angles.Data(0).mean) - delta;
	radius.mean = v.mod();
	angles.Data(0).mean = v.Angle();
}

///calculate total area  
float RingSector::GetArea(){
	float sum = 0;
	int i;
	for(i=0; i<angles.Num_datas(); i++){
		sum += angles.Data(i).delta;
	} 
	sum = 4 * Deg2Rad(sum) * radius.mean * radius.delta;
	return sum;
}

Vector RingSector::GetCenterPos(){
	Vector cpos;
	if(valid){
		cpos = Polar2Vector(radius.mean, angles.Data(0).mean);
	}
	return cpos;
}

void RingSector::operator =(const RingSector &ps){
	valid = ps.IsValid();
	if(!valid)	return;
	radius = ps.radius;
	angles = ps.angles;
}

//点Polar2Vector(angle, dist)离该区域的最近距离
float RingSector::Dist(float angle, float dist){
	float Min_dist = 1000.0f;
	if(!valid) return Min_dist;
	float dif, tmpdif, tmp_dist;
	int i;
	for(i=0; i<angles.Num_datas(); i++){
		dif = AngleDifference(angles.Data(i).mean, angle); 
		tmpdif = dif - angles.Data(i).delta;
		if(tmpdif <= 0){
			if(dist < radius.inf()){
				tmp_dist = radius.inf() - dist;
			}else if(dist > radius.sup()){
				tmp_dist = dist - radius.sup();
			}else
				return 0.0f;
		}else{
			float p = dist * Cos(tmpdif);
			if(p < radius.inf()){
				tmp_dist = (float)sqrt( Sqr(radius.inf()-p) + Sqr(dist*Sin(tmpdif)) );
			}else if(p > radius.sup()){
				tmp_dist = (float)sqrt( Sqr(p-radius.sup()) + Sqr(dist*Sin(tmpdif)) );
			}else
				tmp_dist = dist * Sin(tmpdif);
		}
		Min_dist = Min(Min_dist, tmp_dist);
	}
	return Min_dist;
}

bool RingSector::Isthrough(float angle, float dist){
	if(!valid) return false;
	if(dist < radius.inf()) return false;
	int i;
	for(i=0; i<angles.Num_datas(); i++){
		float dif = AngleDifference(angles.Data(i).mean, angle); 
		if(dif <= angles.Data(i).delta) return true;
	}
	return false;
}

float RingSector::Getmax_x(){
	if(!valid) return 0;
	float min_angle = 180.0;
	int i;
	for(i=0; i<angles.Num_datas(); i++){
		if(NormalizeAngle(0 - angles.Data(i).inf(), 0) < 2 * angles.Data(i).delta){
			min_angle = 0;
			break;
		}
		float tmp0 = (float)fabs(NormalizeAngle(angles.Data(i).inf()));
		float tmp1 = (float)fabs(NormalizeAngle(angles.Data(i).sup()));
		min_angle = Min(min_angle, Min(tmp0, tmp1));
	} 
	if(min_angle <= 90){
		return radius.sup() * Cos(min_angle); 
	}else{
		return radius.inf() * Cos(min_angle);
	}
}

float RingSector::Getmin_x(){
	if(!valid) return 0;
	float max_angle = 0;
	int i;
	for(i=0; i<angles.Num_datas(); i++){
		if(NormalizeAngle(180 - angles.Data(i).inf(), 0) < 2 * angles.Data(i).delta){
			max_angle = 180.0f;
			break;
		}
		
		float tmp0 = (float)fabs(NormalizeAngle(angles.Data(i).inf()));
		float tmp1 = (float)fabs(NormalizeAngle(angles.Data(i).sup()));
		max_angle = Max(max_angle, Max(tmp0, tmp1));
	} 
	if(max_angle <= 90){
		return radius.inf() * Cos(max_angle); 
	}else{
		return radius.sup() * Cos(max_angle);
	}
}

bool RingSector::IsWithin(float angle, float dist)
{
	return IsWithin(angle) && radius.IsWithin(dist);
}