Vector2D Vector2D::rotate( AngDeg angle ){
// determine the polar representation of the current Vector2D
float dMag = this->getLength( );
float dNewDir = this->getDirection( ) + angle; // add rotation angle to phi
setVector2D( dMag, dNewDir, POLAR ); // convert back to Cartesian
return ( *this );
}
void PotentialPlanner2::calcTotalForce()
{
Vector2D temp_force;
setVector2D(temp_force,0.0,0.0);
for (int i=0;i<pedestrians->size();++i)
{
temp_force = addVector2D(temp_force, calcForce( *((*pedestrians)[i]) ));
}
//ADD THE GOAL EFFECT
temp_force.y += 70.0*m_charge;
//ADD THE ROAD EFFECT
dd isInside = 1.0;
dd dx_left = car->getX()-PAVEMENT_LEFT_X_MAX-car->getWidth()/2;
dd dx_right = car->getX()-PAVEMENT_RIGHT_X_MIN+car->getWidth()/2;
if (!(car->getX()-car->getWidth()/2 > PAVEMENT_LEFT_X_MAX && car->getX()+car->getWidth()/2 < PAVEMENT_RIGHT_X_MIN)) isInside=-1.0;
temp_force.x += (10.0*10.0)*isInside*m_charge*abs(car->getV()*cos(car->getTheta()))/(dx_left*dx_left*dx_left);
temp_force.x += (10.0*10.0)*isInside*m_charge*abs(car->getV()*cos(car->getTheta()))/(dx_right*dx_right*dx_right);
//We use temp_force first to let the GUI only drawing the resultant force
m_force = temp_force;
}
Vector2D::Vector2D( float x, float y, CoordSystemT cs ){
setVector2D( x, y, cs );
}
