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));
}
}
}
// 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;
}
}
}
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;
}
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);
}
bool
Collider::isPointInside(const Vec2d& p) const
{
if (distanceTo(p) <= radius_) {
return true;
} else {
return false;
}
}
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);
}
bool
Collider::isColliderInside(const Collider& other) const
{
if ((other.getRadius() > radius_)
|| (distanceTo(other) > radius_ - other.getRadius())) {
return false;
} else {
return true;
}
}
bool
Collider::isColliding(const Collider& other) const
{
double minimumDistance(other.getRadius() + radius_);
if (distanceTo(other) <= minimumDistance) {
return true;
} else {
return false;
}
}
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;
}
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)
}
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));
}
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;
}
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);
}
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);
}
}
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);
}
}
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);
}
double
Collider::distanceTo(const Collider& other) const
{
return distanceTo(other.getPosition());
}
bool isSameAs(StateOfCar const& givenLocation) {
return (distanceTo(givenLocation) < 35);
}
bool Ray::doesIntersect(Sphere& sphere){
return distanceTo( sphere.center ) <= sphere.radius;
}
bool Player::inMyAttackRange(const Player *other) const{
return distanceTo(other) <= attack_range;
}
// 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*/
}
glm::vec3 Ray::intersects( Plane& plane ){
float t = distanceTo( plane );
return at( t );
}
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;
}