diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 784aac9..9e763f5 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -44,7 +44,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer, void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object) { Vector2 worldPosition2 = - Vector2::Rotate(Vector2::forward * object->position.magnitude, + Vector2::Rotate(Vector2::forward * object->position.distance, -object->position.horizontalAngle); Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y); @@ -105,7 +105,8 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { #endif } -void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj) { +void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, + InterestingThing *obj) { #ifdef RC_DEBUG Serial.print("Send Destroy "); Serial.println((int)obj->id); diff --git a/Perception.cpp b/Perception.cpp index 782ca94..1f5e6b4 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -80,7 +80,7 @@ float Perception::GetDistance(float horizontalDirection, float range) { if (obj->position.horizontalAngle > horizontalDirection - range && obj->position.horizontalAngle < horizontalDirection + range) { - minDistance = fminf(minDistance, obj->position.magnitude); + minDistance = fminf(minDistance, obj->position.distance); } } return minDistance; @@ -99,7 +99,7 @@ float Perception::GetDistance(float horizontalDirection, if (obj->position.horizontalAngle > horizontalDirection - range && obj->position.horizontalAngle < horizontalDirection + range) { - minDistance = fminf(minDistance, obj->position.magnitude); + minDistance = fminf(minDistance, obj->position.distance); } } @@ -116,7 +116,7 @@ bool Perception::ObjectNearby(float direction, float range) { if (obj->position.horizontalAngle > direction - range && obj->position.horizontalAngle < direction + range) { - if (obj->position.magnitude <= nearbyDistance) + if (obj->position.distance <= nearbyDistance) return true; } } @@ -146,8 +146,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[objIx]->position.magnitude > - this->trackedObjects[farthestObjIx]->position.magnitude)) { + (this->trackedObjects[objIx]->position.distance > + this->trackedObjects[farthestObjIx]->position.distance)) { farthestObjIx = objIx; } } @@ -166,8 +166,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { } // If this object is closer than the farthest object, then replace it - else if (obj->position.magnitude < - this->trackedObjects[farthestObjIx]->position.magnitude) { + else if (obj->position.distance < + this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; obj->id = availableSlotIx; @@ -208,8 +208,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position) { } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[objIx]->position.magnitude > - this->trackedObjects[farthestObjIx]->position.magnitude)) { + (this->trackedObjects[objIx]->position.distance > + this->trackedObjects[farthestObjIx]->position.distance)) { farthestObjIx = objIx; } } @@ -228,8 +228,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position) { } // If this object is closer than the farthest object, then replace it - else if (obj->position.magnitude < - this->trackedObjects[farthestObjIx]->position.magnitude) { + else if (obj->position.distance < + this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; obj->id = availableSlotIx; @@ -266,9 +266,9 @@ InterestingThing *Perception::GetMostInterestingThing() { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { InterestingThing *obj = this->trackedObjects[objIx]; if (obj != nullptr) { - if (obj->position.magnitude < closestDistance) { + if (obj->position.distance < closestDistance) { closestObject = obj; - closestDistance = obj->position.magnitude; + closestDistance = obj->position.distance; } } } diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 71e3fb1..752e79f 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -19,8 +19,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position) { bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) { if (id != 0 && id == otherObj->id) return true; - if (fabsf(position.magnitude - otherObj->position.magnitude) > - equalityDistance) + if (fabsf(position.distance - otherObj->position.distance) > equalityDistance) return false; if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) > equalityAngle) diff --git a/VectorAlgebra b/VectorAlgebra index 276d293..7810691 160000 --- a/VectorAlgebra +++ b/VectorAlgebra @@ -1 +1 @@ -Subproject commit 276d293c9b02b6d995bc7e951613e94d7a106ac2 +Subproject commit 78106912a9e80c1ace5533615cab0bd4a00657a0