std::vector<Position2D>
NIImporter_OpenDrive::geomFromLine(const OpenDriveEdge &e, const OpenDriveGeometry &g) throw() {
std::vector<Position2D> ret;
ret.push_back(Position2D(g.x, g.y));
ret.push_back(calculateStraightEndPoint(g.hdg, g.length, Position2D(g.x, g.y)));
return ret;
}
void
Line2D::extrapolateBy(SUMOReal length) {
SUMOReal oldlen = myP1.distanceTo(myP2);
SUMOReal x1 = myP1.x() - (myP2.x() - myP1.x()) * (length) / oldlen;
SUMOReal y1 = myP1.y() - (myP2.y() - myP1.y()) * (length) / oldlen;
SUMOReal x2 = myP2.x() - (myP1.x() - myP2.x()) * (length) / oldlen;
SUMOReal y2 = myP2.y() - (myP1.y() - myP2.y()) * (length) / oldlen;
myP1 = Position2D(x1, y1);
myP2 = Position2D(x2, y2);
}
void PhysicSystem::updateFixed()
{
mWorld.Step(mTimeStep, mVelocityIterations, mPositionIterations);
std::map<const GQE::Uint32, std::deque<GQE::IEntity*> >::iterator anIter;
anIter = mEntities.begin();
while(anIter != mEntities.end())
{
std::deque<GQE::IEntity*>::iterator anQueue = anIter->second.begin();
while(anQueue != anIter->second.end())
{
// get the IEntity address first
GQE::IEntity* anEntity = *anQueue;
b2Body* anBody = anEntity->mProperties.get<b2Body*>(BODY);
b2Vec2 anPosition = anBody->GetPosition();
float anAngle = anBody->GetAngle();
anEntity->mProperties.set<Position2D>(POSITION, Position2D(anPosition.x, anPosition.y, anAngle * TODEG));
// Increment the IEntity iterator second
anQueue++;
}
anIter++;
}
}
Position2D GameEntity::calcuateNewPositionOnMovingBackward()
{
float x = this->getPosition().getX() - (_acc * (*_deltaTime)) * (float)sin(this->getINDIEntity()->getAngleZ() / 180.f * PI);
float y = this->getPosition().getY() + (_acc * (*_deltaTime)) * (float)cos(this->getINDIEntity()->getAngleZ() / 180.f * PI);
return Position2D(x, y);
}
void
Line2D::rotateAround(const Position2D &at, SUMOReal rot) {
myP1.add(-at.x(), -at.y());
myP2.add(-at.x(), -at.y());
{
SUMOReal x = myP1.x() * cos(rot) + myP1.y() * sin(rot);
SUMOReal y = myP1.y() * cos(rot) - myP1.x() * sin(rot);
myP1 = Position2D(x, y);
}
{
SUMOReal x = myP2.x() * cos(rot) + myP2.y() * sin(rot);
SUMOReal y = myP2.y() * cos(rot) - myP2.x() * sin(rot);
myP2 = Position2D(x, y);
}
myP1.add(at.x(), at.y());
myP2.add(at.x(), at.y());
}
Position2D
NIImporter_OpenDrive::calculateStraightEndPoint(double hdg, double length, const Position2D &start) throw() {
double normx = 1.0f;
double normy = 0.0f;
double x2 = normx * cos(hdg) - normy * sin(hdg);
double y2 = normx * sin(hdg) + normy * cos(hdg);
normx = x2 * length;
normy = y2 * length;
return Position2D(start.x() + normx, start.y() + normy);
}
std::vector<Position2D>
NIImporter_OpenDrive::geomFromArc(const OpenDriveEdge &e, const OpenDriveGeometry &g) throw() {
std::vector<Position2D> ret;
SUMOReal dist = 0.0;
SUMOReal centerX = g.x;
SUMOReal centerY = g.y;
// left: positive value
SUMOReal curvature = g.params[0];
SUMOReal radius = 1. / curvature;
// center point
calculateCurveCenter(¢erX, ¢erY, radius, g.hdg);
SUMOReal endX = g.x;
SUMOReal endY = g.y;
SUMOReal startX = g.x;
SUMOReal startY = g.y;
SUMOReal hdgS = g.hdg;
SUMOReal hdgE;
SUMOReal geo_posS = g.s;
SUMOReal geo_posE = g.s;
int index1 = 0;
bool end = false;
do {
geo_posE += C_LENGTH;
if (geo_posE - g.s > g.length) {
geo_posE = g.s + g.length;
}
if (geo_posE - g.s > g.length) {
geo_posE = g.s + g.length;
}
calcPointOnCurve(&endX, &endY, centerX, centerY, radius, geo_posE - geo_posS);
dist += (geo_posE - geo_posS);
if (curvature > 0.0) {
hdgE = g.hdg + dist/fabs(radius);
} else {
hdgE = g.hdg - dist/fabs(radius);
}
//
ret.push_back(Position2D(startX, startY));
//
startX = endX;
startY = endY;
geo_posS = geo_posE;
hdgS = hdgE;
if (geo_posE - (g.s + g.length) < 0.001 && geo_posE - (g.s + g.length) > -0.001) {
end = true;
}
} while (!end);
return ret;
}
std::vector<Position2D>
NIImporter_OpenDrive::geomFromSpiral(const OpenDriveEdge &e, const OpenDriveGeometry &g) throw() {
std::vector<Position2D> ret;
SUMOReal curveStart = g.params[0];
SUMOReal curveEnd = g.params[1];
Point2D<double> end;
EulerSpiral s(Point2D<double>(g.x, g.y), g.hdg, curveStart, (curveEnd-curveStart)/g.length, g.length);
std::vector<Point2D<double> > into;
s.computeSpiral(into, 1.);
for (std::vector<Point2D<double> >::iterator i=into.begin(); i!=into.end(); ++i) {
ret.push_back(Position2D((*i).getX(), (*i).getY()));
}
return ret;
}
Position2D
Line2D::getPositionAtDistance(SUMOReal offset) const {
SUMOReal length = myP1.distanceTo(myP2);
if (length==0) {
if (offset!=0) {
throw 1;
}
return myP1;
}
SUMOReal x = myP1.x() + (myP2.x() - myP1.x()) / length * offset;
SUMOReal y = myP1.y() + (myP2.y() - myP1.y()) / length * offset;
/* SUMOReal x2 = myP2.x() - (myP1.x() - myP2.x()) / length * offset;
SUMOReal y2 = myP2.y() - (myP1.y() - myP2.y()) / length * offset;*/
return Position2D(x, y);
}
Position2D
NLGeomShapeBuilder::getPointPosition(SUMOReal x, SUMOReal y,
const std::string &laneID,
SUMOReal posOnLane) const throw(InvalidArgument) {
if (x!=INVALID_POSITION&&y!=INVALID_POSITION) {
return Position2D(x,y);
}
MSLane *lane = MSLane::dictionary(laneID);
if (lane==0) {
throw InvalidArgument("Lane '" + laneID + "' to place a poi on is not known.");
}
if (posOnLane<0) {
posOnLane = lane->getLength() + posOnLane;
}
return lane->getShape().positionAtLengthPosition(posOnLane);
}
void PhysicSystem::handleInit(GQE::IEntity* theEntity)
{
GQE::IEntity* aFather = theEntity->mProperties.get<GQE::IEntity*>(PARENT);
b2Body* aBody = 0;
if (aFather)
{
b2Body* aFatherBody = aFather->mProperties.get<b2Body*>(BODY);
b2JointDef* aJointDef = theEntity->mProperties.get<b2JointDef*>(JOINTDEF);
if(aJointDef)
{
switch(aJointDef->type)
{
case b2JointType::e_weldJoint:
{
GQE::typePropertyID aPropertyName = theEntity->mProperties.get<GQE::typePropertyID>(ANCHOR_POINT);
b2Vec2 aFatherAnchorPoint = aFather->mProperties.get<b2Vec2>(aPropertyName);
b2WeldJointDef* aWeldJointDef = static_cast<b2WeldJointDef*>(aJointDef);
aWeldJointDef->localAnchorA = aFatherAnchorPoint;
b2Vec2 aPosition = aFatherBody->GetPosition() + aFatherAnchorPoint + aWeldJointDef->localAnchorB;
theEntity->mProperties.set<Position2D>(POSITION, Position2D(aPosition.x, aPosition.y, 0));
break;
}
default:
break;
}
aBody = setBody(theEntity);
aJointDef->bodyA = aFatherBody;
aJointDef->bodyB = aBody;
b2Joint* aJoint = mWorld.CreateJoint(aJointDef);
theEntity->mProperties.set<b2Joint*>(JOINT, aJoint);
}
}
else
{
aBody = setBody(theEntity);
}
aBody->SetActive(true);
}
void GameStateOne::handleEvents(sf::Event theEvent)
{
// Exit program if Escape key is pressed
if((theEvent.type == sf::Event::KeyReleased) &&
(theEvent.key.code == sf::Keyboard::Escape))
{
mApp.quit(GQE::StatusAppOK);
}
else if((theEvent.type == sf::Event::KeyReleased) &&
(theEvent.key.code == sf::Keyboard::B))
{
if(mPlayer)
{
deactivateEntity( mPlayer->mProperties.get<GQE::IEntity*>(ACTOR));
}
}
else if((theEvent.type == sf::Event::KeyReleased) &&
(theEvent.key.code == sf::Keyboard::A))
{
}
else
{
for(int i = 0; i < 8; i++)
{
if(sf::Joystick::isConnected(i) && sf::Joystick::isButtonPressed(i, 0))
if(!mPlayer)
{
mPlayer = addPlayer(i, BASIC_SHIP_PROTO, Position2D(40, 160, 90 * TORAD));
GQE::IEntity* actor = mPlayer->mProperties.get<GQE::IEntity*>(ACTOR);
GQE::Prototype* propeller = mPrototypes.getPrototype(SHIP_PROPELLER_PROTO);
propeller->mProperties.add<GQE::IEntity*>(PARENT, actor);
propeller->makeInstance();
GQE::Prototype* prototype = mPrototypes.getPrototype(MACHINEGUN_PROTO);
prototype->mProperties.add<GQE::IEntity*>(PARENT, actor);
prototype->mProperties.set<GQE::typePropertyID>(ANCHOR_POINT, LEFT_ANCHOR);
prototype->makeInstance();
prototype->mProperties.set<GQE::typePropertyID>(ANCHOR_POINT, RIGHT_ANCHOR);
prototype->makeInstance();
}
}
}
}
bool
NIVissimSingleTypeParser_Laengenverteilungsdefinition::parse(std::istream &from) {
// id
std::string id;
from >> id;
// list of points
Position2DVector points;
std::string tag;
do {
tag = readEndSecure(from);
if (tag!="DATAEND") {
SUMOReal p1 = TplConvert<char>::_2SUMOReal(tag.c_str());
from >> tag;
SUMOReal p2 = TplConvert<char>::_2SUMOReal(tag.c_str());
points.push_back(Position2D(p1, p2));
}
} while (tag!="DATAEND");
NBDistribution::dictionary("length",
id, new Distribution_Points(id, points));
return true;
}
void PhysicSystem::addProperties(GQE::IEntity* theEntity)
{
theEntity->mProperties.add<Position2D>(POSITION, Position2D(0, 0, 0));
theEntity->mProperties.add<b2Body*>(BODY, 0);
theEntity->mProperties.add<bool>(INDEPENDENT, false);
}
Point2Di OccupancyMapParams::getDeltaPixelFromDeltaPosition(
const Position2D &position) const {
// Give an offset of (0.0, 0.0) to get the delta.
return getPixelFromOffsetAndScale(AL::Math::pose2DFromPosition2D(position),
metersPerPixel, Position2D());
}
Position2D OccupancyMapParams::getPositionFromPixel(
const Point2Di &pixel) const {
return Position2D(pixel.x * metersPerPixel + originOffset.x,
-pixel.y * metersPerPixel + originOffset.y);
}
