Exemple #1
0
void Snake::Player::update() {
  GameObject::update();
  double speed_delta = speed * time_delta;
  next_node->speed_delta = speed_delta;
  switch (direction) {
    case left:
      x -= speed_delta;
      break;
    case right:
      x += speed_delta;
      break;
    case up:
      y -= speed_delta;
      break;
    case down:
      y += speed_delta;
      break;
  }

  if (next_node->collidesWith(this, 0)) {
    // TODO: implement game over
    speed = 0;
  }

  if (!next_node->path.empty()) {
    PathPoint* last_path = next_node->path.back();
    if (distanceTo(last_path->x, last_path->y) > speed_delta) {
      next_node->addPath(new PathPoint(x, y));
    }
  } else {
    if (distanceTo(next_node->x, next_node->y) > speed_delta) {
      next_node->addPath(new PathPoint(x, y));
    }
  }
}
Exemple #2
0
// UPDATE
void Entity::Update(float elapsed, Entity* e)
{
	vel_x = lerp(vel_x, 0.0, elapsed * 2.0);
	vel_x += acc_x * elapsed;

	if (type == PLAYER)
	{
		if (shootCooldown > 0.0)
		{
			shootCooldown -= elapsed;
		}
	}
	
	else if (type == GRUNT)
	{
		if (distanceTo(e) <= 12.0)
		{
			moveTo_x(e->x);
			move_x(elapsed);
			willShoot = true;
		}
		if (shootCooldown > 0.0)
		{
			shootCooldown -= elapsed;
		}
	}
	
}
Exemple #3
0
double Navigator::bearingTo(Point& mapPoint ) {

    Point platformPoint = convertMapToPlatform(mapPoint);
    Point bodyPoint = convertPlatformToBody(platformPoint);
    double head2 = asin(bodyPoint.y/distanceTo(mapPoint) );
    return head2;
}
/**
  Define interaction between 2 degrees of freedoms  (real case)
 */
void interaction_real(void* data, int i, int j, void* result)
{
  problem_data_t* pdata = (problem_data_t*) data;
  double* points = pdata->points;
  double r = distanceTo(&points[3*i], &points[3*j]);

  *((double*)result) = exp(-fabs(r) / pdata->l);
}
//ceci n'est pas une méthode statique
valarray<double> MassiveParticle::compacc(MassiveParticle part2) const {
    double r(0), intensity(0);
    valarray<double> dir(0.,3), acc(0.,3);
    r = distanceTo(part2);
    dir = directionTo(part2);
    intensity = - G * part2.m_mass / (r*r);
    acc = dir * intensity;
    return acc;
}
Exemple #6
0
Point2d Point2d::rulerPoint(const Point2d& dir, float yoff) const
{
    float len = distanceTo(dir);
    if (len < _MGZERO) {
        return Point2d(x, y + yoff);
    }
    yoff /= len;
    return Point2d(x - (dir.y - y) * yoff, y + (dir.x - x) * yoff);
}
Exemple #7
0
bool
Collider::isPointInside(const Vec2d& p) const
{
    if (distanceTo(p) <= radius_) {
        return true;
    } else {
        return false;
    }
}
Exemple #8
0
Point2d Point2d::rulerPoint(const Point2d& dir, float xoff, float yoff) const
{
    float len = distanceTo(dir);
    if (len < _MGZERO)
        return Point2d(x + xoff, y + yoff);
    float dcos = (dir.x - x) / len;
    float dsin = (dir.y - y) / len;
    return Point2d(x + xoff * dcos - yoff * dsin,
                   y + xoff * dsin + yoff * dcos);
}
Exemple #9
0
bool
Collider::isColliderInside(const Collider& other) const
{
    if ((other.getRadius() > radius_)
        || (distanceTo(other) > radius_ - other.getRadius())) {
        return false;
    } else {
        return true;
    }
}
Exemple #10
0
bool
Collider::isColliding(const Collider& other) const
{
    double minimumDistance(other.getRadius() + radius_);

    if (distanceTo(other) <= minimumDistance) {
        return true;
    } else {
        return false;
    }
}
Exemple #11
0
bool Player::canSlash(const Player *other, bool distance_limit) const{
    if(other->hasSkill("kongcheng") && other->isKongcheng())
        return false;

    if(other == this)
        return false;

    if(distance_limit)
        return distanceTo(other) <= getAttackRange();
    else
        return true;
}
Exemple #12
0
void goToPoint()
{
	update();
	
	printf("VPS Current Point: (%d, %d)\n", game.coords[0].x, game.coords[0].y);
	if(currentPt.y > 0){
		desiredPt.x = 3.3;
		desiredPt.y = 2.5;
	}
	else{
		desiredPt.x = 3.3;
		desiredPt.y = -2.6;
	}
	
	printf("Current Point: (%f, %f)\n", currentPt.x, currentPt.y);
	printf("Desired Point: (%f, %f)\n", desiredPt.x, desiredPt.y);
	printf("Our current heading is: %f\n", (float)game.coords[0].theta/2047*180); //NEED TO CONVERT TO DEGREES
	float targetHeading = getTargetTheta(desiredPt);
	printf("The target heading is: %f\n", targetHeading);
	pointTurn(targetHeading);
	
	float dist = distanceTo(desiredPt);
	printf("The distance is: %f\n", dist);

	
	while(dist > TOLERANCE)
	{
		driveStraight(dist);
		printf("Trying to drive straight\n");
		update();
		dist = distanceTo(desiredPt);
	}
	brake();
	
	//get location data
	//determine angle and position relative to desired point
	//turn to face desired point
	//drive "straight" to point (unless obstacles)
}
Exemple #13
0
 Vector4 CSGCubeSource::getValueAndGradient(const Vector3 &position) const
 { 
     // Just approximate the normal via a simple Prewitt. To get the real normal, 26 cases had to be evaluated...
     Vector3 gradient(getValue(Vector3(position.x + (Real)1.0, position.y, position.z)) - getValue(Vector3(position.x - (Real)1.0, position.y, position.z)),
         getValue(Vector3(position.x, position.y + (Real)1.0, position.z)) - getValue(Vector3(position.x, position.y - (Real)1.0, position.z)),
         getValue(Vector3(position.x, position.y, position.z + (Real)1.0)) - getValue(Vector3(position.x, position.y, position.z - (Real)1.0)));
     gradient.normalise();
     gradient *= (Real)-1.0;
     return Vector4(
         gradient.x,
         gradient.y,
         gradient.z,
         distanceTo(position));
 }
Exemple #14
0
void
Engine::goTorward(float x, float y)
{
    std::string myError;
    std::ostringstream X;
    std::ostringstream Y;
    X << x;
    Y << y;
    myError = "Cannot reach destination (" + X.str() +", " + Y.str() + ").";
    if (distanceTo(x, y) > _power)
        throw UserError(myError, "Engine");
    _x = x;
    _y = y;
}
Exemple #15
0
bool Monster::targetClosestEnemy() {
	auto& game = server.game();
	auto position = getPosition();

	SpectatorList spectators;
	game.getSpectators(spectators, position);

	if (spectators.empty()) {
		target(nullptr);
		return false;
	}

	auto closestDistance = std::numeric_limits<uint32_t>::max();
	CreatureP closestSpectator;

	for (auto spectator : spectators) {
		if (!isEnemy(*spectator)) {
			continue;
		}

		if (!canAttack(*spectator)) {
			continue;
		}

		auto spectatorPosition = spectator->getPosition();
		if (!game.isSightClear(position, spectatorPosition, true)) {
			continue;
		}

		auto distance = position.distanceTo(spectatorPosition);
		if (distance < closestDistance) {
			closestSpectator = spectator;
			if (distance == 1) {
				break;
			}

			closestDistance = distance;
		}
	}

	if (closestSpectator == nullptr) {
		target(nullptr);
		return false;
	}

	return target(closestSpectator);
}
Exemple #16
0
void driveStraight(float dist)
{
	float timeStart = get_time();
	Point prevPt = {currentPt.x, currentPt.y};

	motor_vel = speedLimit(KD*dist);
	straight(motor_vel);
	pause(500);
	update();
	if (distanceTo(prevPt) < 0.01)  //working too hard, what to do?
	{
		brake();
		wiggle(5);
		straight(70); //backward at speed 70
		pause(1000);
		pointTurn(0);
	}
}
Exemple #17
0
SearchNode* SearchNode::expandTowards(SearchNode* dest)
{

    float dx,dy,dtheta,mag;
    dx = dest->x() - this->xVal;
    dy = dest->y() - this->yVal;
    dtheta = dest->theta() - this->thetaVal;
    mag = distanceTo(dest);
    if(mag < stepSize)
    {
        return new SearchNode(dest->x(),dest->y(),dest->theta(),this);
    }
    else
    {
        return new SearchNode(this->xVal + (stepSize*dx/mag),
                        this->yVal + (stepSize*dy/mag),
                        this->thetaVal + (stepSize*dtheta/mag), this);
    }
}
Exemple #18
0
void LOD::update( const Camera::Ptr& camera ) const {

  auto v1 = Vector3();
  auto v2 = Vector3();

  if ( objects.size() > 1 ) {

    v1.setFromMatrixPosition( camera->matrixWorld );
    v2.setFromMatrixPosition( matrixWorld );

    float distance = v1.distanceTo( v2 );

    objects[ 0 ].object->visible = true;

    size_t i, l;

    for ( i = 1, l = objects.size(); i < l; i ++ ) {

      if ( distance >= objects[ i ].distance ) {

        objects[ i - 1 ].object->visible = false;
        objects[ i     ].object->visible = true;

      } else {

        break;

      }

    }

    for( ; i < l; i ++ ) {

      objects[ i ].object->visible = false;

    }

  }

}
double* createRhs(double *points, int n, double l) {
  double* rhs = (double*) calloc(n,  sizeof(double));
  int i;
  double center[3];
  center[0] = 0.;
  center[1] = 0.;
  center[2] = 0.;

  for (i = 0; i < n; i++) {
      center[0] += points[3*i+0];
      center[1] += points[3*i+1];
      center[2] += points[3*i+2];
  }
  center[0] /= n;
  center[1] /= n;
  center[2] /= n;

  for (i = 0; i < n; i++) {
      double r = distanceTo(center, &points[3*i]);
      rhs[i] = exp(-fabs(r) / l);
  }
  return rhs;
}
void trafficlights::acquire_target()
{

    QList<QGraphicsItem *> colliding_items = area->collidingItems();
    if(colliding_items.size()==1){
        has_vehicle = false;
        return;

    }
    double closest_dist = 300;
    QPointF closest_pt = QPointF(0,0);
    for(size_t i=0, n= colliding_items.size();i<n;i++){
        Vehicle * vehicle = dynamic_cast<Vehicle*>(colliding_items[i]);
        if(vehicle){ //if isnt vehicle type it will be a null ptr and will never execute
           double this_dist = distanceTo(vehicle);
                closest_dist = this_dist;
                closest_pt = colliding_items[i]->pos();
                has_vehicle = true;

        }
    }
   destination = closest_pt;
}
        inline bool isCloseTo(StateOfCar const& givenLocation) {

            return (distanceTo(givenLocation) < 100);
        }
Exemple #22
0
double
Collider::distanceTo(const Collider& other) const
{
    return distanceTo(other.getPosition());
}
        bool isSameAs(StateOfCar const& givenLocation) {

            return (distanceTo(givenLocation) < 35);
        }
Exemple #24
0
 bool Ray::doesIntersect(Sphere& sphere){
     return distanceTo( sphere.center ) <= sphere.radius;
 }
Exemple #25
0
bool Player::inMyAttackRange(const Player *other) const{
    return distanceTo(other) <= attack_range;
}
Exemple #26
0
// Fill _results with all of the results in the annulus defined by _innerRadius and
// _outerRadius.  If no results are found, grow the annulus and repeat until success (or
// until the edge of the world).
void S2NearIndexCursor::fillResults() {
    verify(_results.empty());
    if (_innerRadius >= _outerRadius) {
        return;
    }
    if (_innerRadius > _maxDistance) {
        return;
    }

    // We iterate until 1. our search radius is too big or 2. we find results.
    do {
        // Some of these arguments are opaque, look at the definitions of the involved classes.
        FieldRangeSet frs(_descriptor->parentNS().c_str(), makeFRSObject(), false, false);
        shared_ptr<FieldRangeVector> frv(new FieldRangeVector(frs, _specForFRV, 1));
        scoped_ptr<BtreeCursor> cursor(BtreeCursor::make(nsdetails(_descriptor->parentNS()),
                                       _descriptor->getOnDisk(), frv, 0, 1));

        // The cursor may return the same obj more than once for a given
        // FRS, so we make sure to only consider it once in any given annulus.
        //
        // We don't want this outside of the 'do' loop because the covering
        // for an annulus may return an object whose distance to the query
        // point is actually contained in a subsequent annulus.  If we
        // didn't consider every object in a given annulus we might miss
        // the point.
        //
        // We don't use a global 'seen' because we get that by requiring
        // the distance from the query point to the indexed geo to be
        // within our 'current' annulus, and I want to dodge all yield
        // issues if possible.
        unordered_set<DiskLoc, DiskLoc::Hasher> seen;

        LOG(1) << "looking at annulus from " << _innerRadius << " to " << _outerRadius << endl;
        LOG(1) << "Total # returned: " << _stats._numReturned << endl;
        // Do the actual search through this annulus.
        for (; cursor->ok(); cursor->advance()) {
            // Don't bother to look at anything we've returned.
            if (_returned.end() != _returned.find(cursor->currLoc())) {
                ++_stats._returnSkip;
                continue;
            }

            ++_stats._nscanned;
            if (seen.end() != seen.find(cursor->currLoc())) {
                ++_stats._btreeDups;
                continue;
            }

            // Get distance interval from our query point to the cell.
            // If it doesn't overlap with our current shell, toss.
            BSONObj currKey(cursor->currKey());
            BSONObjIterator it(currKey);
            BSONElement geoKey;
            for (int i = 0; i <= _nearFieldIndex; ++i) {
                geoKey = it.next();
            }

            S2Cell keyCell = S2Cell(S2CellId::FromString(geoKey.String()));
            if (!_annulus.MayIntersect(keyCell)) {
                ++_stats._keyGeoSkip;
                continue;
            }

            // We have to add this document to seen *AFTER* the key intersection test.
            // A geometry may have several keys, one of which may be in our search shell and one
            // of which may be outside of it.  We don't want to ignore a document just because
            // one of its covers isn't inside this annulus.
            seen.insert(cursor->currLoc());

            // At this point forward, we will not examine the document again in this annulus.

            const BSONObj& indexedObj = cursor->currLoc().obj();

            // Match against indexed geo fields.
            ++_stats._geoMatchTested;
            size_t geoFieldsMatched = 0;
            // See if the object actually overlaps w/the geo query fields.
            for (size_t i = 0; i < _indexedGeoFields.size(); ++i) {
                BSONElementSet geoFieldElements;
                indexedObj.getFieldsDotted(_indexedGeoFields[i].getField(), geoFieldElements,
                                           false);
                if (geoFieldElements.empty()) {
                    continue;
                }

                bool match = false;

                for (BSONElementSet::iterator oi = geoFieldElements.begin();
                        !match && (oi != geoFieldElements.end()); ++oi) {
                    if (!oi->isABSONObj()) {
                        continue;
                    }
                    const BSONObj &geoObj = oi->Obj();
                    GeometryContainer geoContainer;
                    uassert(16762, "ill-formed geometry: " + geoObj.toString(),
                            geoContainer.parseFrom(geoObj));
                    match = _indexedGeoFields[i].satisfiesPredicate(geoContainer);
                }

                if (match) {
                    ++geoFieldsMatched;
                }
            }

            if (geoFieldsMatched != _indexedGeoFields.size()) {
                continue;
            }

            // Get all the fields with that name from the document.
            BSONElementSet geoFieldElements;
            indexedObj.getFieldsDotted(_nearQuery.field, geoFieldElements, false);
            if (geoFieldElements.empty()) {
                continue;
            }

            ++_stats._inAnnulusTested;
            double minDistance = 1e20;
            // Look at each field in the document and take the min. distance.
            for (BSONElementSet::iterator oi = geoFieldElements.begin();
                    oi != geoFieldElements.end(); ++oi) {
                if (!oi->isABSONObj()) {
                    continue;
                }
                double dist = distanceTo(oi->Obj());
                minDistance = min(dist, minDistance);
            }

            // We could be in an annulus, yield, add new points closer to
            // query point than the last point we returned, then unyield.
            // This would return points out of order.
            if (minDistance < _returnedDistance) {
                continue;
            }

            // If the min. distance satisfies our distance criteria
            if (minDistance >= _innerRadius && minDistance < _outerRadius) {
                // The result is valid.  We have to de-dup ourselves here.
                if (_returned.end() == _returned.find(cursor->currLoc())) {
                    _results.push(Result(cursor->currLoc(), cursor->currKey(),
                                         minDistance));
                }
            }
        }

        if (_results.empty()) {
            LOG(1) << "results empty!\n";
            _radiusIncrement *= 2;
            nextAnnulus();
        } else if (_results.size() < 300) {
            _radiusIncrement *= 2;
        } else if (_results.size() > 600) {
            _radiusIncrement /= 2;
        }
    } while (_results.empty()
             && _innerRadius < _maxDistance
             && _innerRadius < _outerRadius);
    LOG(1) << "Filled shell with " << _results.size() << " results" << endl;
}
    void GoalieExtension::run(void* msg)
    {
        /*PROTECTED REGION ID(run1459249216387) ENABLED START*/ //Add additional options here
        auto ownPos = wm->rawSensorData->getOwnPositionVision();
        if (ownPos == nullptr)
            return;

        auto ballPos = wm->ball->getEgoBallPosition();
        if (ballPos == nullptr)
            return;

        //kick
        msl_actuator_msgs::KickControl km;
        long currentTime = wm->getTime();
        if (currentTime - lastKickerTime >= KICKER_WAIT_TIME)
        {
            if (useKicker == true && ballPos != nullptr && ballPos->length() < 420
                    && (abs(ballPos->angleTo()) - M_PI) < 0.52)
            {

                km.enabled = true;
                km.kicker = 1;
                km.power = 100;
                send(km);
                lastKickerTime = wm->getTime();
            }
        }
        if (wm->rawSensorData->getLastMotionCommand() == nullptr)
            return;
        bm_last = msl_actuator_msgs::MotionControl();
        bm_last.motion.translation = wm->rawSensorData->getLastMotionCommand()->motion.translation;
        bm_last.motion.rotation = wm->rawSensorData->getLastMotionCommand()->motion.rotation;
        bm_last.motion.angle = wm->rawSensorData->getLastMotionCommand()->motion.angle;
        auto ballPosAllo = ballPos->egoToAllo(*ownPos);
        long now = wm->getTime() / 1000000;
        auto ballPos3D = wm->ball->getBallPoint3D();
        if (ballPos3D != nullptr)
        {
            if (ballPos3D->z > 500)
            {
                ballInAirTimestamp = now;
            }
        }

        auto ballV3D = wm->ball->getBallVel3D();
        if (ballV3D != nullptr && ballPos != nullptr)
        {

            auto ballV3DAllo = ballV3D->egoToAllo(*ownPos);
            double velo = sqrt(ballV3D->x * ballV3D->x + ballV3D->y * ballV3D->y + ballV3D->z * ballV3D->z);
            if (velo > 1000)
            {

                double diffAngle = abs(wm->rawSensorData->getLastMotionCommand()->motion.angle - bm_last.motion.angle);
                double diffTrans = abs(
                        wm->rawSensorData->getLastMotionCommand()->motion.translation - bm_last.motion.translation);
                double diffRot = abs(
                        wm->rawSensorData->getLastMotionCommand()->motion.rotation - bm_last.motion.rotation);
                double distBall = ballPos->length();

                double speed = wm->rawSensorData->getLastMotionCommand()->motion.translation;
                double g = 0;

                double g1 = 1.0 / (1.0 + exp(0.03 * (speed - 100.0))); //speed
                double g2 = 1.0 / (1.0 + exp(10 * (diffAngle - 0.5)));
                g2 = 1;
                double g3 = 1.0 / (1.0 + exp(0.03 * (diffTrans - 100)));
                g3 = 1;

                double g4 = 1.0 / (1.0 + exp(10 * (diffRot - 100)));
                double g5 = 1.0 / (1.0 + exp(0.0006 * (distBall - 6000)));
                g = g1 * g2 * g3 * g4 * g5;

                shared_ptr < geometry::CNPoint3D > ballVelo3DAllo = make_shared < geometry::CNPoint3D
                        > (ballV3DAllo->x, ballV3DAllo->y, ballV3DAllo->z);

                double x = -wm->field->getFieldLength() / 2 + 100;
                double timeBallToGoal = (x - ballPosAllo->x) / ballVelo3DAllo->x;
                //Console.WriteLine("ghgh timeBallToGoal " + timeBallToGoal);
                if (timeBallToGoal > 0)
                {
                    double y = ballPosAllo->y + ballVelo3DAllo->y * timeBallToGoal;
                    if (abs(y) < wm->field->getGoalWidth() / 2 + 2000)
                    {
                        if (y > wm->field->getGoalWidth() / 2)
                            y = wm->field->getGoalWidth() / 2;
                        if (y < -wm->field->getGoalWidth() / 2)
                            y = -wm->field->getGoalWidth() / 2;
                        auto dstPoint = make_shared < geometry::CNPoint2D > (x, y);
                        auto dstPointEgo = dstPoint->alloToEgo(*ownPos);

                        auto lookAt = make_shared < geometry::CNPoint2D > (0, 0);
                        auto lookAtEgo = lookAt->alloToEgo(*ownPos);
                        double lookAtAngle = lookAtEgo->angleTo() + M_PI;

                        ballVelo3DAllo = ballVelo3DAllo->normalize();

                        ballGoalProjection->overWrite(dstPoint, g);
                        ballVelocity->overWrite(make_shared < geometry::CNPoint2D > (ballV3DAllo->x, ballV3DAllo->y),
                                                g);

                        int count = (int)(ballPos->length() * ballPos->length()) / 1000000;
                        if (count > 3)
                        {
                            count = 3;
                        }
                        dstPoint = ballGoalProjection->getAvgPoint(count);
                        dstPointEgo = dstPoint->alloToEgo(*ownPos);

                        auto ballVeloBuf = ballVelocity->getAvgPoint((int)(ballPos->length() - 2000) / 2000);
                        double veloBuf = sqrt(ballVeloBuf->x * ballVeloBuf->x + ballVeloBuf->y * ballVeloBuf->y);
                        double timeBallToGoal2 = ballPosAllo->distanceTo(wm->field->posOwnGoalMid()) / veloBuf;
                        double timeToDstPoint = dstPointEgo->length() / 800;
                        if (timeToDstPoint > timeBallToGoal2 && ballPos->length() < 5000)
                        {
                            if (useExt3 == true && dstPointEgo->angleTo() < 0)
                            {
                                km.extension = msl_actuator_msgs::KickControl::LEFT_EXTENSION;
                            }
                            else if (useExt2 == true)
                            {
                                km.extension = msl_actuator_msgs::KickControl::RIGHT_EXTENSION;
                            }
                            if (useExt3 == true || useExt2 == true)
                            {
                                km.extTime = 1000;
                                send(km);
                            }
                        }
                        else if (useExt1 == true && timeBallToGoal2 < 1.0 && abs(dstPointEgo->y - ownPos->y) < 400
                                && ballInAirTimestamp + 3000 > now)
                        {
                            km.extension = msl_actuator_msgs::KickControl::UPPER_EXTENSION;
                            km.extTime = 1000;
                            send(km);
                        }

                    }
                }
            }
        }

        /*PROTECTED REGION END*/
    }
Exemple #28
0
 glm::vec3 Ray::intersects( Plane& plane ){
     float t = distanceTo( plane );
     return at( t );
 }
Exemple #29
0
bool Player::inMyAttackRange(const Player *other) const{
    return distanceTo(other) <= getAttackRange();
}
int main()
{
    int ret = 0;

    {
        Entity tlve("0", 0), ent("1", 1);

        ent.m_location.m_loc = &tlve;
        ent.m_location.m_pos = Point3D(1, 1, 0);
        ent.m_location.m_orientation = WFMath::Quaternion().identity();

        Point3D relPos = relativePos(ent.m_location, ent.m_location);

        std::cout << "RelPos to self: " << relPos << std::endl << std::flush;

        relPos = relativePos(ent.m_location, tlve.m_location);

        std::cout << "RelPos ent -> tlve: " << relPos
                  << std::endl << std::flush;

        relPos = relativePos(tlve.m_location, ent.m_location);

        std::cout << "RelPos tlve -> ent: " << relPos
                  << std::endl << std::flush;

        ent.m_location.m_loc = 0;
    }

    {
        Entity tlve("0", 0), ent1("1", 1), ent2("2", 2);

        ent1.m_location.m_loc = &tlve;
        ent1.m_location.m_pos = Point3D(-1, 1, 0);
        ent1.m_location.m_orientation = WFMath::Quaternion().identity();

        ent2.m_location.m_loc = &tlve;
        ent2.m_location.m_pos = Point3D(1, 1, 0);
        ent2.m_location.m_orientation = WFMath::Quaternion().identity();

        Point3D relPos = relativePos(ent1.m_location, ent2.m_location);

        std::cout << "RelPos ent1 -> ent2: " << relPos
                  << std::endl << std::flush;

        ent1.m_location.m_loc = 0;
        ent2.m_location.m_loc = 0;
    }

    {
        Entity tlve("0", 0), ent1("1", 1), ent2("2", 2),
               ent3("3", 3), ent4("4", 4);

        ent1.m_location.m_loc = &tlve;
        ent1.m_location.m_pos = Point3D(-1, 1, 0);
        ent1.m_location.m_orientation = WFMath::Quaternion().identity();

        ent2.m_location.m_loc = &tlve;
        ent2.m_location.m_pos = Point3D(1, 1, 0);
        ent2.m_location.m_orientation = WFMath::Quaternion().identity();

        ent3.m_location.m_loc = &ent1;
        ent3.m_location.m_pos = Point3D(-1, 1, 0);
        ent3.m_location.m_orientation = WFMath::Quaternion().identity();

        ent4.m_location.m_loc = &ent2;
        ent4.m_location.m_pos = Point3D(1, 1, 0);
        ent4.m_location.m_orientation = WFMath::Quaternion().identity();

        Point3D relPos = relativePos(ent3.m_location, ent4.m_location);

        std::cout << "RelPos ent3 -> ent4: " << relPos
                  << std::endl << std::flush;

        ent1.m_location.m_loc = 0;
        ent2.m_location.m_loc = 0;
        ent3.m_location.m_loc = 0;
        ent4.m_location.m_loc = 0;
    }

    {
        Entity tlve("0", 0), ent1("1", 1), ent2("2", 2), ent3("3", 3), ent4("4", 4);

        ent1.m_location.m_loc = &tlve;
        ent1.m_location.m_pos = Point3D(-1, 1, 0);
        ent1.m_location.m_orientation = WFMath::Quaternion().identity();

        ent2.m_location.m_loc = &tlve;
        ent2.m_location.m_pos = Point3D(1, 1, 0);
        ent2.m_location.m_orientation = WFMath::Quaternion().identity();

        ent3.m_location.m_loc = &ent1;
        ent3.m_location.m_pos = Point3D(-1, 1, 0);
        ent3.m_location.m_orientation = WFMath::Quaternion(2, M_PI / 2.f);

        ent4.m_location.m_loc = &ent2;
        ent4.m_location.m_pos = Point3D(1, 1, 0);
        ent4.m_location.m_orientation = WFMath::Quaternion().identity();

        Point3D relPos = relativePos(ent3.m_location, ent4.m_location);

        std::cout << "RelPos ent3 -> ent4: " << relPos
                  << std::endl << std::flush;

        ent1.m_location.m_loc = 0;
        ent2.m_location.m_loc = 0;
        ent3.m_location.m_loc = 0;
        ent4.m_location.m_loc = 0;
    }

    {
        Entity tlve("0", 0), ent1("1", 1), ent2("2", 2),
               ent3("3", 3), ent4("4", 4);

        ent1.m_location.m_loc = &tlve;
        ent1.m_location.m_pos = Point3D(-1, 1, 0);
        ent1.m_location.m_orientation = WFMath::Quaternion().identity();

        ent2.m_location.m_loc = &tlve;
        ent2.m_location.m_pos = Point3D(1, 1, 0);
        ent2.m_location.m_orientation = WFMath::Quaternion(2, -M_PI / 2.f);

        ent3.m_location.m_loc = &ent1;
        ent3.m_location.m_pos = Point3D(-1, 1, 0);
        ent3.m_location.m_orientation = WFMath::Quaternion(2, M_PI / 2.f);

        ent4.m_location.m_loc = &ent2;
        ent4.m_location.m_pos = Point3D(1, 1, 0);
        ent4.m_location.m_orientation = WFMath::Quaternion().identity();

        Point3D relPos = relativePos(ent3.m_location, ent4.m_location);

        std::cout << "RelPos ent3 -> ent4: " << relPos
                  << std::endl << std::flush;

        ent1.m_location.m_loc = 0;
        ent2.m_location.m_loc = 0;
        ent3.m_location.m_loc = 0;
        ent4.m_location.m_loc = 0;
    }

    {
        Entity tlve("0", 0), ent1("1", 1), ent2("2", 2),
               ent3("3", 3), ent4("4", 4);

        ent1.m_location.m_loc = &tlve;
        ent1.m_location.m_pos = Point3D(-1, 1, 0);
        ent1.m_location.m_orientation = WFMath::Quaternion(2, M_PI / 2.f);

        ent2.m_location.m_loc = &tlve;
        ent2.m_location.m_pos = Point3D(1, 1, 0);
        ent2.m_location.m_orientation = WFMath::Quaternion().identity();

        ent3.m_location.m_loc = &ent1;
        ent3.m_location.m_pos = Point3D(-1, 1, 0);
        ent3.m_location.m_orientation = WFMath::Quaternion(2, M_PI / 2.f);

        ent4.m_location.m_loc = &ent2;
        ent4.m_location.m_pos = Point3D(1, 1, 0);
        ent4.m_location.m_orientation = WFMath::Quaternion().identity();

        Point3D relPos = relativePos(ent3.m_location, ent4.m_location);
        Vector3D distance = distanceTo(ent3.m_location, ent4.m_location);

        std::cout << "RelPos ent3 -> ent4: " << relPos
                  << " Distance ent3 -> ent4: " << distance
                  << std::endl << std::flush;

        ent1.m_location.m_loc = 0;
        ent2.m_location.m_loc = 0;
        ent3.m_location.m_loc = 0;
        ent4.m_location.m_loc = 0;
    }

    {
        Entity tlve("0", 0), ent1("1", 1), ent2("2", 2);

        ent1.m_location.m_loc = &tlve;
        ent1.m_location.m_pos = Point3D(1, 1, 0);
        ent1.m_location.m_orientation = WFMath::Quaternion().identity();

        ent2.m_location.m_loc = &ent1;
        ent2.m_location.m_pos = Point3D(0, 0, 0);
        ent2.m_location.m_orientation = WFMath::Quaternion().identity();

        Vector3D distance = distanceTo(ent1.m_location, ent2.m_location);

        std::cout << "Distance ent1 -> ent2: "
                  << distance << "," << distance.isValid()
                  << std::endl << std::flush;

        assert(distance.isValid());
        assert(distance == Vector3D(0,0,0));
        ent1.m_location.m_loc = 0;
        ent2.m_location.m_loc = 0;
    }
    return ret;
}