C++ Point::Rotate方法代码示例

	/*
	 * 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;
}

//.........这里部分代码省略.........
        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);
 }