/*
* the intersections of two circle
*/
vector<Point> Intersection(const Circle &rhs) const {
double d = (O - rhs.O).Abs();
double cost = (R * R + d * d - rhs.R * rhs.R) / (2.0 * R * d);
double sint = sqrt(1.0 - cost * cost);
Point rfn = (rhs.O - O) / d * R;
return {O + rfn.Rotate(cost, sint), O + rfn.Rotate(cost, -sint)};
}
// return spacing and id of the nearest pedestrian
my_pair VelocityModel::GetSpacing(Pedestrian* ped1, Pedestrian* ped2, Point ei, int periodic) const
{
Point distp12 = ped2->GetPos() - ped1->GetPos(); // inversed sign
if(periodic){
double x = ped1->GetPos()._x;
double x_j = ped2->GetPos()._x;
if((xRight-x) + (x_j-xLeft) <= cutoff){
distp12._x = distp12._x + xRight - xLeft;
}
}
double Distance = distp12.Norm();
double l = 2*ped1->GetEllipse().GetBmax();
Point ep12;
if (Distance >= J_EPS) {
ep12 = distp12.Normalized();
} else {
//printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
Log->Write("WARNING: \tin VelocityModel::GetSPacing() ep12 can not be calculated!!!\n");
Log->Write("\t\t Pedestrians are too near to each other (%f).", Distance);
exit(EXIT_FAILURE);
}
double condition1 = ei.ScalarProduct(ep12); // < e_i , e_ij > should be positive
double condition2 = ei.Rotate(0, 1).ScalarProduct(ep12); // theta = pi/2. condition2 should <= than l/Distance
condition2 = (condition2>0)?condition2:-condition2; // abs
if((condition1 >=0 ) && (condition2 <= l/Distance))
// return a pair <dist, condition1>. Then take the smallest dist. In case of equality the biggest condition1
return my_pair(distp12.Norm(), ped2->GetID());
else
return my_pair(FLT_MAX, ped2->GetID());
}
/*****************************************************************************
*** ProjectionBasedAbsoluteDirectionCalculus (PBADC)
*****************************************************************************/
int getPBADC_Relation( int m, Point ptOrigin, Point ptRelatum, float angleRelative) {
Point origin( 0,0);
ptRelatum.Shift( origin-ptOrigin );
ptRelatum.Rotate( -angleRelative, origin );
BB_DBG(3) << origin << " : " << ptRelatum << endl;
return calculateOrientationSegmentOPRA( m, origin.GetAngleTo( ptRelatum ) );
};
std::vector< int > getOPRA_Relation2(int m,
Point ptA, double oriA,
Point ptB, double oriB) {
int relAB, relBA;
std::vector< int > result;
CAngle cangleA(oriA);
CAngle cangleB(oriB);
BB_DBG(2) << "Punkt A: " << ptA << ":" << cangleA << " & Punkt B: " << ptB << ":" << cangleB << endl;
ptB.Rotate( -cangleA.get(), ptA );
cangleB = cangleB - cangleA;
BB_DBG(2) << "Punkt A: " << ptA << ":" << cangleA-cangleA << " & Punkt B: " << ptB << ":" << cangleB << endl;
CAngle cangleAB = ptA.GetAngleTo( ptB );
CAngle cangleBA = ptB.GetAngleTo( ptA );
cangleBA -= cangleB;
BB_DBG(2) << "Relative Angle: (AB:" << cangleAB << ") & (BA:" << cangleBA << ")" << endl;
relAB = calculateOrientationSegmentOPRA( m, cangleAB.get() );
relBA = calculateOrientationSegmentOPRA( m, cangleBA.get() );
if( ptA == ptB ) {
result.push_back( relBA );
} else {
result.push_back( relAB );
result.push_back( relBA );
}
return result;
}
Point CArc::MidParam(double param)const {
/// returns a point which is 0-1 along arc
if(fabs(param) < 0.00000000000001)return m_s;
if(fabs(param - 1.0) < 0.00000000000001)return m_e;
Point p;
Point v = m_s - m_c;
v.Rotate(param * IncludedAngle());
p = v + m_c;
return p;
}
int GPCC::calculateTheta2( Point ptReferent ) {
if( !m_bOriginSet || !m_bRelatumSet) { // insufficient data 4 calc
return false;
}
Point ptOrigin( m_ptOrigin );
Point ptRelatum( m_ptRelatum );
float correctionAngle = 0.0;
float fErrorEpsilon = 0.0001;
Point ptTmp(0,0);
// shift all 3 pt's by (-relatum) => relatum =[0,0]
ptOrigin.Shift( ptTmp-m_ptRelatum );
ptRelatum.Shift( ptTmp-m_ptRelatum );
ptReferent.Shift( ptTmp-m_ptRelatum );
BB_DBG(5) << "SHIFTED Points: "
<< ptOrigin << ptRelatum << ptReferent
<< endl;
// rotate ori & ref, such that ori=[-m_fRadius,0]
// ptTmp.SetXY( -m_fRadius, 0 );
// correctionAngle = ptOrigin.GetAngleTo( ptTmp );
correctionAngle = -ptOrigin.GetAngleTo( ptRelatum );
ptOrigin.Rotate( correctionAngle );
ptReferent.Rotate( correctionAngle );
BB_DBG(5) << "ROTATED Points by " << correctionAngle << " rad: "
<< ptOrigin << ptRelatum << ptReferent
<< endl;
if( (ptOrigin.Y()>fErrorEpsilon) || (ptOrigin.Y()<-fErrorEpsilon) ) {
cout << "Y Coordinate of Origin is not correct!!!!!!!" << endl;
exit(1);
}
if( ptOrigin.X() >= 0 ) {
cout << "Origin is on wrong side of coordinate system!!!!!" << endl;
exit(1);
}
// Get the actual angle between the three points
m_fTheta = ptRelatum.GetAngleTo( ptReferent );
// ptTmp.SetXY( m_fDistRelRef, 0);
// m_fTheta = ptReferent.GetAngleTo( ptTmp );
m_aTheta.set( m_fTheta );
BB_DBG(5) << "Angle between [Origin, Relatum, Referent] = "
<< m_ptOrigin << ", "
<< m_ptRelatum << ", "
<< ptReferent << "] = "
<< m_fTheta << " ( " << m_aTheta << ")" << endl;
return true;
}
Point Span::MidPerim(double d)const {
/// returns a point which is 0-d along span
Point p;
if(m_v.m_type == 0) {
Point vs = m_v.m_p - m_p;
vs.normalize();
p = vs * d + m_p;
}
else {
Point v = m_p - m_v.m_c;
double radius = v.length();
v.Rotate(d * m_v.m_type / radius);
p = v + m_v.m_c;
}
return p;
}
Point Span::MidParam(double param)const {
/// returns a point which is 0-1 along span
if(fabs(param) < 0.00000000000001)return m_p;
if(fabs(param - 1.0) < 0.00000000000001)return m_v.m_p;
Point p;
if(m_v.m_type == 0) {
Point vs = m_v.m_p - m_p;
p = vs * param + m_p;
}
else {
Point v = m_p - m_v.m_c;
v.Rotate(param * IncludedAngle());
p = v + m_v.m_c;
}
return p;
}
std::vector< char > getDC_Relation( int m,
Point ptStartA,
Point ptEndA,
Point ptStartB,
Point ptEndB) {
std::vector< int > pre_result;
std::vector< char > result;
std::vector< Line > lines;
std::vector< Point > points;
Line lineA(ptStartA, ptEndA);
Line lineB(ptStartB, ptEndB);
lines.push_back( lineA );
lines.push_back( lineA );
lines.push_back( lineB );
lines.push_back( lineB );
points.push_back( ptStartB );
points.push_back( ptEndB );
points.push_back( ptStartA );
points.push_back( ptEndA );
pre_result.push_back( lineA.GetSide(ptStartB) );
pre_result.push_back( lineA.GetSide(ptEndB) );
pre_result.push_back( lineB.GetSide(ptStartA) );
pre_result.push_back( lineB.GetSide(ptEndA) );
// if DRA no 3 points are allowed on a line
// DRA: r,l
// DRA_c: r,l & e,s
// DRA_f: r,l & e,s & b,i,f
// DRA_fp: r,l & e,s & b,i,f + 5th position: A,P,+,-
for( unsigned int index=0; index<4; index++) {
// calculate relation char
int side = lines[index].GetSide( points[index] );
if( side == (-1)) {
result.push_back( 'l' );
} else if( side == 1) {
result.push_back( 'r' );
} else { // =0 => on line
if( lines[index].ptStart == points[index] )
result.push_back( 's' );
else if( lines[index].ptEnd == points[index] )
result.push_back( 'e' );
else {
int OnLine = lines[index].PositionOnLineSegment( points[index] );
if( lines[index].PositionOnLineSegment( points[index] ) == (-1))
result.push_back( 'b' );
else if( lines[index].PositionOnLineSegment( points[index] ) == 0)
result.push_back( 'i' );
else if( lines[index].PositionOnLineSegment( points[index] ) == (1))
result.push_back( 'f' );
else {
cout << "( :error )" << endl;
exit(-1);
}
}
}
}
// Restrictions for the specific Dipole cases
if( m==DRA ) {
for( unsigned int i=0; i<4; i++) {
if(result[i]=='s' || result[i]=='e' || result[i]=='b' || result[i]=='i' || result[i]=='f' ) {
result.clear();
}
}
}
if( m==DRA_c ) {
for( unsigned int i=0; i<4; i++) {
if( result[i]=='b' || result[i]=='i' || result[i]=='f' ) {
result.clear();
}
}
}
// if( m==DRA ) { // nothing to be done
// }
if( m==DRA_fp ) {
if( lineA.IsParallelTo( lineB )) {
string str;
for( int i=0; i<result.size();i++ ) {
str += result[i];
if( str=="sese" || str=="ffbb" || str=="efbs" || str=="ifbi" ||
str=="bfii" || str=="sfsi" || str=="beie" || str=="bbff" ||
str=="bsef" || str=="biif" || str=="iibf" || str=="sisf" ||
str=="iebe" || str=="rrll" || str=="llrr" ) {
result.push_back( 'P' ); // Allen cases
}
if( str=="eses" || str=="ffff" || str=="fefe" || str=="fifi" ||
str=="fbii" || str=="fsei" || str=="ebis" || str=="iifb" ||
str=="eifs" || str=="iseb" || str=="bbbb" || str=="sbsb" ||
str=="ibib" || str=="rrrr" || str=="llll") {
result.push_back( 'A' ); // Converse Allen cases
}
}
} else {
// Point ptCut;
// if( lineA.IsParallelTo( lineB )) {
// // now is the question if A or P
// if( lineA == lineB ) {
// string str;
// for( int i=0; i<result.size();i++ ) {
// str += result[i];
// }
// result.push_back( '*' );
// } else {
// Line lineX( ptStartA, ptStartB);
// Line lineY( ptEndA, ptEndB);
// if( lineX.GetCut( lineY, ptCut ) <= 0 ) { // no cut => P
// result.push_back( 'A' );
// } else {
// result.push_back( 'P' );
// }
// }
// } else {
// // calculate mathematical orientation
// if( lineB.GetAngle()-lineA.GetAngle() > 0 ) {
// result.push_back( '-' );
// } else { // lineB.GetAngle()-lineA.GetAngle() < 0
// result.push_back( '+' );
// }
// }
// Shift all points by -sA
Point ptOrigin( 0,0);
//cout << "Line A: " << ptStartA << ptEndA << " & Line B: " << ptStartB << ptEndB << endl;
ptEndA.Shift( ptOrigin - ptStartA );
ptStartB.Shift( ptOrigin - ptStartA );
ptEndB.Shift( ptOrigin - ptStartA );
ptStartA.Shift( ptOrigin - ptStartA );
//cout << "Line A: " << ptStartA << ptEndA << " & Line B: " << ptStartB << ptEndB << endl;
// Rotate all points by -phi(eA)
float angle = ptStartA.GetAngleTo( ptEndA );
ptStartA.Rotate( -angle );
ptEndA.Rotate( -angle );
ptStartB.Rotate( -angle );
ptEndB.Rotate( -angle );
//cout << "Line A: " << ptStartA << ptEndA << " & Line B: " << ptStartB << ptEndB << endl;
// Shift eB by -sB
ptEndB.Shift( ptOrigin - ptStartB );
ptStartB.Shift( ptOrigin - ptStartB );
//cout << "Line A: " << ptStartA << ptEndA << " & Line B: " << ptStartB << ptEndB << endl;
// if(ptEndB.Y()>-0.0001 && ptEndB.Y()<0.0001) {
// // lines are parallel => A or P (epsilon due to shift/rotate errors)
// if( ptEndB.X()>0 ) {
// result.push_back( 'P' );
// } else if( ptEndB.X()<0 ) {
// result.push_back( 'A' );
// } else {
// result.push_back( '*' );
// }
// } else
if(ptEndB.Y()>0) {
result.push_back( '+' );
} else if(ptEndB.Y()<0) {
result.push_back( '-' );
}
}
}
return result;
}
Line move(const double &d){
Point tmp = b - a;
tmp = tmp / tmp.Abs();
tmp = tmp.Rotate(PI/2);
return Line(a+tmp*d, b+tmp*d);
}