Compare commits
No commits in common. "main" and "master" have entirely different histories.
5
.gitignore
vendored
5
.gitignore
vendored
@ -1,3 +1,2 @@
|
||||
build
|
||||
.vscode
|
||||
DoxyGen/DoxyWarnLogfile.txt
|
||||
doxygen/html
|
||||
doxygen/DoxyWarnLogfile.txt
|
||||
|
||||
3
.gitmodules
vendored
Normal file
3
.gitmodules
vendored
Normal file
@ -0,0 +1,3 @@
|
||||
[submodule "VectorAlgebra"]
|
||||
path = VectorAlgebra
|
||||
url = http://gitlab.passervr.com/passer/cpp/vectoralgebra.git
|
||||
41
Accelerometer.h
Normal file
41
Accelerometer.h
Normal file
@ -0,0 +1,41 @@
|
||||
#pragma once
|
||||
|
||||
#include "Sensor.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A Sensor which can measure the magnetic field
|
||||
class Accelerometer : public Sensor {
|
||||
public:
|
||||
Accelerometer(){};
|
||||
|
||||
/// @brief Returns the direction of the acceleration in the horizontal plane
|
||||
/// @return The direction of the acceleration, negative is left, positive is
|
||||
/// right
|
||||
/// @note The horizontal plane is defined as being orthogonal to the gravity
|
||||
/// vector
|
||||
/// @note the units (degrees, radians, -1..1, ...) depend on the sensor
|
||||
float GetHorizontalAccelerationDirection();
|
||||
/// @brief Returns the magnitude of the acceleration in the horizontal plane
|
||||
/// @return The magnitude. This value is never negative.
|
||||
/// @note the unity (m/s^2, 0..1) depends on the sensor.
|
||||
float GetHorizontalAccelerationMagnitude();
|
||||
|
||||
/// @brief This gives the gravity direciton relative to the down direction.
|
||||
/// @param horizontal the horizontal direction, negative is left, positive is
|
||||
/// right
|
||||
/// @param vertical the vertical direction, negative is down, positive is up
|
||||
/// @note The horizontal plane is defined as being orthogonal to the gravity
|
||||
/// vector
|
||||
/// @note the units (degrees, radians, -1..1, ...) depend on the sensor
|
||||
void GetAccelerationDirection(float* horizontal, float* vertical);
|
||||
/// @brief The magnitude of the gravity field.
|
||||
/// @return The magnitude. This value is never negative.
|
||||
/// @note the unity (m/s^2, 0..1) depends on the sensor.
|
||||
float GetAccelerationMagnitude();
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
46
Activation.cpp
Normal file
46
Activation.cpp
Normal file
@ -0,0 +1,46 @@
|
||||
#include "Activation.h"
|
||||
|
||||
float Activation::HeavisideStep(float inputValue, float bias) {
|
||||
return (inputValue + bias > 0) ? 1.0F : 0.0F;
|
||||
}
|
||||
|
||||
float Activation::Tanh(float inputValue) {
|
||||
return (exp(inputValue) - exp(-inputValue)) / (exp(inputValue) + exp(-inputValue));
|
||||
}
|
||||
|
||||
float Activation::Sigmoid(float inputValue) {
|
||||
return 1 / (1 + expf(-inputValue));
|
||||
}
|
||||
|
||||
float Activation::Linear(float inputValue, float minValue, float range) {
|
||||
if (inputValue > minValue + range)
|
||||
return 0;
|
||||
if (inputValue < minValue)
|
||||
return 1;
|
||||
|
||||
float f = (inputValue - minValue) * (1 / range); // normalize to 1..0
|
||||
float influence = 1 - f; // invert
|
||||
return influence;
|
||||
}
|
||||
|
||||
|
||||
float Activation::Quadratic(float inputValue, float minValue, float range) {
|
||||
if (inputValue > minValue + range)
|
||||
return 0;
|
||||
if (inputValue < minValue)
|
||||
return 1;
|
||||
|
||||
float f = (inputValue - minValue) * (1 / range); // normalize to 1..0
|
||||
float influence = 1 - (f * f); // quadratic & invert
|
||||
return influence;
|
||||
}
|
||||
|
||||
float Activation::ParticleLife(float minValue, float maxValue, float attraction, float inputValue) {
|
||||
if (inputValue < minValue)
|
||||
return inputValue / minValue - 1;
|
||||
|
||||
if (inputValue < maxValue)
|
||||
return attraction * (1 - fabs(2 * inputValue - minValue - maxValue) / (maxValue - minValue));
|
||||
|
||||
return 0;
|
||||
}
|
||||
33
Activation.h
Normal file
33
Activation.h
Normal file
@ -0,0 +1,33 @@
|
||||
#ifndef RC_ACTIVATION_H
|
||||
#define RC_ACTIVATION_H
|
||||
|
||||
#include <math.h>
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
class Activation {
|
||||
public:
|
||||
static float HeavisideStep(float inputValue, float bias = 0); // Range: {0,1}
|
||||
|
||||
static float Tanh(float inputValue); // Range: (-1, 1)
|
||||
|
||||
static float Sigmoid(float inputValue); // Range: (0, 1)
|
||||
|
||||
static float Linear(float inputValue, float bias = 0, float range = 0);
|
||||
|
||||
static float Quadratic(float inputValue,
|
||||
float bias = 0,
|
||||
float range = 0); // minValue = bias
|
||||
|
||||
static float ParticleLife(float minValue,
|
||||
float maxValue,
|
||||
float attraction,
|
||||
float inputValue); // minValue = bias
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
|
||||
#endif
|
||||
@ -1,126 +0,0 @@
|
||||
#include "ArduinoParticipant.h"
|
||||
|
||||
#if defined(ARDUINO)
|
||||
#if defined(ARDUINO_ARCH_ESP8266)
|
||||
#include <ESP8266WiFi.h>
|
||||
|
||||
#elif defined(ESP32)
|
||||
#include <WiFi.h>
|
||||
|
||||
#elif defined(UNO_R4)
|
||||
#include <WiFi.h>
|
||||
|
||||
#elif defined(ARDUINO_ARCH_RP2040) // not functional, for future use
|
||||
#include <WifiNINA.h>
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
void LocalParticipant::Setup(int localPort,
|
||||
const char* remoteIpAddress,
|
||||
int remotePort) {
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
this->remoteIpAddress = remoteIpAddress;
|
||||
this->remotePort = remotePort;
|
||||
GetBroadcastAddress();
|
||||
|
||||
#if defined(UNO_R4)
|
||||
if (WiFi.status() == WL_NO_MODULE) {
|
||||
std::cout << "No network available!\n";
|
||||
return;
|
||||
}
|
||||
#else
|
||||
if (WiFi.isConnected() == false) {
|
||||
std::cout << "No network available!\n";
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
udp.begin(localPort);
|
||||
|
||||
std::cout << "Wifi sync started to port " << this->remotePort << "\n";
|
||||
#endif
|
||||
}
|
||||
|
||||
void LocalParticipant::GetBroadcastAddress() {
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
IPAddress broadcastAddress = WiFi.localIP();
|
||||
broadcastAddress[3] = 255;
|
||||
String broadcastIpString = broadcastAddress.toString();
|
||||
this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
|
||||
broadcastIpString.toCharArray(this->broadcastIpAddress,
|
||||
broadcastIpString.length() + 1);
|
||||
std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
|
||||
#endif
|
||||
}
|
||||
|
||||
void LocalParticipant::Receive() {
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
int packetSize = udp.parsePacket();
|
||||
while (packetSize > 0) {
|
||||
udp.read(buffer, packetSize);
|
||||
|
||||
String senderAddress = udp.remoteIP().toString();
|
||||
char sender_ipAddress[16];
|
||||
senderAddress.toCharArray(sender_ipAddress, 16);
|
||||
unsigned int sender_port = udp.remotePort();
|
||||
|
||||
// Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
|
||||
// sender_port); if (remoteParticipant == nullptr) {
|
||||
// remoteParticipant = this->AddParticipant(sender_ipAddress,
|
||||
// sender_port);
|
||||
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
|
||||
// // << "\n";
|
||||
// // std::cout << "New remote participant " <<
|
||||
// remoteParticipant->ipAddress
|
||||
// // << ":" << remoteParticipant->port << " "
|
||||
// // << (int)remoteParticipant->networkId << "\n";
|
||||
// }
|
||||
|
||||
// ReceiveData(packetSize, remoteParticipant);
|
||||
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
||||
packetSize = udp.parsePacket();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
|
||||
// << remoteParticipant->port << "\n";
|
||||
|
||||
int n = 0;
|
||||
do {
|
||||
if (n > 0) {
|
||||
std::cout << "Retry sending\n";
|
||||
delay(10);
|
||||
}
|
||||
n++;
|
||||
udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
|
||||
udp.write((unsigned char*)buffer, bufferSize);
|
||||
} while (udp.endPacket() == 0 && n < 10);
|
||||
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool LocalParticipant::Publish(IMessage* msg) {
|
||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||
int bufferSize = msg->Serialize((char*)this->buffer);
|
||||
if (bufferSize <= 0)
|
||||
return true;
|
||||
|
||||
udp.beginPacket(this->broadcastIpAddress, this->remotePort);
|
||||
udp.write((unsigned char*)buffer, bufferSize);
|
||||
udp.endPacket();
|
||||
|
||||
// std::cout << "Publish to " << this->broadcastIpAddress << ":"
|
||||
// << this->remotePort << "\n";
|
||||
#endif
|
||||
return true;
|
||||
};
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,32 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "../LocalParticipant.h"
|
||||
|
||||
#if defined(HAS_WIFI)
|
||||
#include <WiFiUdp.h>
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
class LocalParticipant : public RoboidControl::LocalParticipant {
|
||||
public:
|
||||
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
||||
void Receive();
|
||||
bool Send(Participant* remoteParticipant, int bufferSize);
|
||||
bool Publish(IMessage* msg);
|
||||
|
||||
protected:
|
||||
#if defined(HAS_WIFI)
|
||||
const char* remoteIpAddress = nullptr;
|
||||
unsigned short remotePort = 0;
|
||||
char* broadcastIpAddress = nullptr;
|
||||
|
||||
WiFiUDP udp;
|
||||
#endif
|
||||
|
||||
void GetBroadcastAddress();
|
||||
};
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,218 +0,0 @@
|
||||
#include "ArduinoUtils.h"
|
||||
|
||||
#if defined(ARDUINO)
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#if defined(ARDUINO_ARCH_ESP8266)
|
||||
#include <ESP8266WiFi.h>
|
||||
#include <ESP8266httpUpdate.h>
|
||||
#include <HTTPClient.h>
|
||||
|
||||
#elif defined(ESP32)
|
||||
#include <HTTPClient.h>
|
||||
#include <HTTPUpdate.h>
|
||||
#include <WiFi.h>
|
||||
|
||||
#elif defined(UNO_R4)
|
||||
#include <WiFi.h>
|
||||
|
||||
#endif
|
||||
|
||||
const char* hotspotSSID = "Roboid";
|
||||
const char* hotspotPassword = "alchemy7000";
|
||||
|
||||
#if ESP32
|
||||
// Flash storage
|
||||
#include "Preferences.h"
|
||||
|
||||
#define PREFERENCES_NAMESPACE "roboidControl"
|
||||
Preferences wifiPreferences;
|
||||
|
||||
#define STORAGE_KEY_WIFI "rc/wifi"
|
||||
struct WifiCredentials {
|
||||
char ssid[32] = "\0";
|
||||
char password[32] = "\0";
|
||||
} credentials;
|
||||
|
||||
#define STORAGE_KEY_NSS "rc/nss"
|
||||
struct NssServer {
|
||||
char ipAddress[16] = "127.0.0.1\0";
|
||||
unsigned short port = 7681;
|
||||
} nssServer;
|
||||
#endif
|
||||
|
||||
bool StartWifi(const char* wifiSsid,
|
||||
const char* wifiPassword,
|
||||
bool hotspotFallback) {
|
||||
#if !defined(HAS_WIFI)
|
||||
return false;
|
||||
#else
|
||||
#if defined(UNO_R4) || defined(ARDUINO_ARCH_RP2040)
|
||||
if (WiFi.status() == WL_NO_MODULE) {
|
||||
Serial.println("WiFi not present, WiFiSync is disabled");
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if ESP32
|
||||
printf("Connecting to WiFi %s\n", wifiSsid);
|
||||
#else
|
||||
Serial.print("Connecting to WiFi ");
|
||||
Serial.println(wifiSsid);
|
||||
#endif
|
||||
|
||||
// Connect to Wifi
|
||||
WiFi.begin(wifiSsid, wifiPassword);
|
||||
uint32_t notConnectedCounter = 0;
|
||||
bool connected = false;
|
||||
bool hotSpotEnabled = false;
|
||||
|
||||
while (WiFi.status() != WL_CONNECTED && !hotSpotEnabled) {
|
||||
#if ESP32
|
||||
printf(".");
|
||||
#else
|
||||
Serial.print(".");
|
||||
#endif
|
||||
delay(500);
|
||||
|
||||
notConnectedCounter++;
|
||||
if (notConnectedCounter > 20 && hotspotFallback) {
|
||||
#if ESP32
|
||||
printf("\nCould not connect to home network.\n");
|
||||
#else
|
||||
Serial.println();
|
||||
Serial.println("Could not connect to home network");
|
||||
#endif
|
||||
WiFi.disconnect();
|
||||
if (hotspotFallback) {
|
||||
#if ESP32
|
||||
WiFi.mode(WIFI_OFF);
|
||||
WiFi.mode(WIFI_AP);
|
||||
IPAddress wifiMyIp(192, 168, 4, 1);
|
||||
WiFi.softAPConfig(wifiMyIp, wifiMyIp, IPAddress(255, 255, 255, 0));
|
||||
|
||||
WiFi.softAP(hotspotSSID, hotspotPassword);
|
||||
#elif UNO_R4 || ARDUINO_ARCH_RP2040
|
||||
WiFi.beginAP(hotspotSSID);
|
||||
#endif
|
||||
printf("Setup WiFi hotspot...\n");
|
||||
// printf("ssid = %s, password = %s\n", hotspotSSID, hotspotPassword);
|
||||
#if ARDUINO_ARCH_RP2040
|
||||
String ipAddress = WiFi.localIP().toString();
|
||||
#else
|
||||
String ipAddress = WiFi.softAPIP().toString();
|
||||
#endif
|
||||
char buf[20];
|
||||
ipAddress.toCharArray(buf, 20);
|
||||
printf("IP address: %s\n", buf);
|
||||
hotSpotEnabled = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
connected = notConnectedCounter <= 20;
|
||||
|
||||
if (connected) {
|
||||
char buf[20];
|
||||
String ipAddress = WiFi.localIP().toString();
|
||||
ipAddress.toCharArray(buf, 20);
|
||||
#if ESP32 || ESP8266
|
||||
printf("\nWifi connected, IP address: %s\n", buf);
|
||||
#else
|
||||
Serial.println();
|
||||
Serial.println("Wifi connected");
|
||||
#endif
|
||||
#if ESP32
|
||||
printf("Checking credentials in flash\n");
|
||||
wifiPreferences.begin(PREFERENCES_NAMESPACE);
|
||||
wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials,
|
||||
sizeof(credentials));
|
||||
if (strcmp(wifiSsid, credentials.ssid) != 0 ||
|
||||
strcmp(wifiPassword, credentials.password) != 0) {
|
||||
printf("Updating credentials in flash...");
|
||||
const int ssidLen = strlen(wifiSsid);
|
||||
if (ssidLen < 32) {
|
||||
memcpy(credentials.ssid, wifiSsid, ssidLen);
|
||||
credentials.ssid[ssidLen] = '\0';
|
||||
}
|
||||
|
||||
const int pwdLen = strlen(wifiPassword);
|
||||
if (pwdLen < 32) {
|
||||
memcpy(credentials.password, wifiPassword, pwdLen);
|
||||
credentials.password[pwdLen] = '\0';
|
||||
}
|
||||
wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
|
||||
sizeof(credentials));
|
||||
printf(" completed.\n");
|
||||
}
|
||||
wifiPreferences.end();
|
||||
#endif
|
||||
}
|
||||
|
||||
return (!hotSpotEnabled);
|
||||
#endif
|
||||
}
|
||||
|
||||
void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
|
||||
#if !defined(HAS_WIFI)
|
||||
return;
|
||||
#else
|
||||
#if defined(UNO_R4) // Uno R4 Wifi does not support this kind of firmware
|
||||
// update (as far as I know)
|
||||
return;
|
||||
#else
|
||||
Serial.println("Checking for firmware updates.");
|
||||
|
||||
WiFiClient client;
|
||||
HTTPClient httpClient;
|
||||
String versionURL = url + FIRMWARE_NAME + ".version";
|
||||
httpClient.begin(client, versionURL);
|
||||
int httpCode = httpClient.GET();
|
||||
if (httpCode == 200) {
|
||||
String newFWVersion = httpClient.getString();
|
||||
|
||||
Serial.print("Current firmware version: ");
|
||||
Serial.println(FIRMWARE_VERSION);
|
||||
Serial.print("Available firmware version: ");
|
||||
Serial.println(newFWVersion);
|
||||
|
||||
int newVersion = newFWVersion.toInt();
|
||||
|
||||
if (newVersion > FIRMWARE_VERSION) {
|
||||
Serial.println("Preparing to update firmware.");
|
||||
|
||||
String firmwareURL = url + FIRMWARE_NAME + ".bin";
|
||||
#if defined(ESP32)
|
||||
t_httpUpdate_return ret = httpUpdate.update(client, firmwareURL);
|
||||
#else
|
||||
t_httpUpdate_return ret = ESPhttpUpdate.update(client, firmwareURL);
|
||||
#endif
|
||||
switch (ret) {
|
||||
case HTTP_UPDATE_FAILED:
|
||||
#if defined(ESP32)
|
||||
Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
|
||||
httpUpdate.getLastError(),
|
||||
httpUpdate.getLastErrorString().c_str());
|
||||
#else
|
||||
Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
|
||||
ESPhttpUpdate.getLastError(),
|
||||
ESPhttpUpdate.getLastErrorString().c_str());
|
||||
#endif
|
||||
break;
|
||||
case HTTP_UPDATE_NO_UPDATES:
|
||||
Serial.println("HTTP_UPDATE_NO_UPDATES");
|
||||
break;
|
||||
case HTTP_UPDATE_OK:
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
Serial.println("No Firmware update necessary.");
|
||||
}
|
||||
} else {
|
||||
Serial.print("Http Error: ");
|
||||
Serial.println(httpCode);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
@ -1,10 +0,0 @@
|
||||
#pragma once
|
||||
#if defined(ARDUINO)
|
||||
|
||||
#include <Arduino.h>
|
||||
bool StartWifi(const char *wifiSsid, const char *wifiPassword,
|
||||
bool hotspotFallback = false);
|
||||
|
||||
void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION);
|
||||
|
||||
#endif
|
||||
@ -1,120 +0,0 @@
|
||||
#include "DRV8833.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
|
||||
: Thing(participant) {
|
||||
this->pinIn1 = pinIn1;
|
||||
this->pinIn2 = pinIn2;
|
||||
|
||||
#if (ESP32)
|
||||
in1Ch = nextAvailablePwmChannel++;
|
||||
ledcSetup(in1Ch, 500, 8);
|
||||
ledcAttachPin(pinIn1, in1Ch);
|
||||
in2Ch = nextAvailablePwmChannel++;
|
||||
ledcSetup(in2Ch, 500, 8);
|
||||
ledcAttachPin(pinIn2, in2Ch);
|
||||
#else
|
||||
pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode
|
||||
pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode
|
||||
#endif
|
||||
|
||||
this->reverse = reverse;
|
||||
}
|
||||
|
||||
void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
|
||||
this->maxRpm = rpm;
|
||||
}
|
||||
|
||||
void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
|
||||
Thing::SetAngularVelocity(velocity);
|
||||
// ignoring rotation axis for now.
|
||||
// Spherical angularVelocity = this->GetAngularVelocity();
|
||||
float angularSpeed = velocity.distance; // in degrees/sec
|
||||
|
||||
float rpm = angularSpeed / 360 * 60;
|
||||
float motorSpeed = rpm / this->maxRpm;
|
||||
|
||||
uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
|
||||
// if (direction == RotationDirection::CounterClockwise)
|
||||
// speed = -speed;
|
||||
// Determine the rotation direction
|
||||
if (velocity.direction.horizontal.InDegrees() < 0)
|
||||
motorSpeed = -motorSpeed;
|
||||
if (this->reverse)
|
||||
motorSpeed = -motorSpeed;
|
||||
|
||||
// std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm " << rpm
|
||||
// << ", motor signal = " << (int)motorSignal << "\n";
|
||||
|
||||
#if (ESP32)
|
||||
if (motorSpeed == 0) { // stop
|
||||
ledcWrite(in1Ch, 0);
|
||||
ledcWrite(in2Ch, 0);
|
||||
|
||||
} else if (motorSpeed > 0) { // forward
|
||||
#if FAST_DECAY
|
||||
ledcWrite(in1Ch, motorSignal);
|
||||
ledcWrite(in2Ch, 0);
|
||||
#else
|
||||
ledcWrite(in1Ch, 255);
|
||||
ledcWrite(in2Ch, 255 - motorSignal);
|
||||
#endif
|
||||
} else { // (motorSpeed < 0) reverse
|
||||
#if FAST_DECAY
|
||||
ledcWrite(in1Ch, 0);
|
||||
ledcWrite(in2Ch, motorSignal);
|
||||
#else
|
||||
ledcWrite(in1Ch, 255 - motorSignal);
|
||||
ledcWrite(in2Ch, 255);
|
||||
#endif
|
||||
}
|
||||
#else // not ESP32
|
||||
if (motorSpeed == 0) { // stop
|
||||
analogWrite(pinIn1, 0);
|
||||
analogWrite(pinIn2, 0);
|
||||
|
||||
} else if (motorSpeed > 0) { // forward
|
||||
#if FAST_DECAY
|
||||
analogWrite(pinIn1, motorSignal);
|
||||
analogWrite(pinIn2, 0);
|
||||
#else
|
||||
analogWrite(pinIn1, 255);
|
||||
analogWrite(pinIn2, 255 - motorSignal);
|
||||
#endif
|
||||
} else { // (motorSpeed < 0) reverse
|
||||
#if FAST_DECAY
|
||||
analogWrite(pinIn1, 0);
|
||||
analogWrite(pinIn2, motorSignal);
|
||||
#else
|
||||
analogWrite(pinIn1, 255 - motorSignal);
|
||||
analogWrite(pinIn2, 255);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
DRV8833::DRV8833(Participant* participant,
|
||||
unsigned char pinAIn1,
|
||||
unsigned char pinAIn2,
|
||||
unsigned char pinBIn1,
|
||||
unsigned char pinBIn2,
|
||||
unsigned char pinStandby,
|
||||
bool reverseA,
|
||||
bool reverseB)
|
||||
: Thing(participant) {
|
||||
this->pinStandby = pinStandby;
|
||||
if (pinStandby != 255)
|
||||
pinMode(pinStandby, OUTPUT);
|
||||
|
||||
this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
|
||||
this->motorA->name = "Motor A";
|
||||
this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
|
||||
this->motorB->name = "Motor B";
|
||||
}
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,60 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Thing.h"
|
||||
#include "Things/DifferentialDrive.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
/// @brief Support for a DRV8833 motor controller
|
||||
class DRV8833Motor : public Thing {
|
||||
public:
|
||||
/// @brief Motor turning direction
|
||||
enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
|
||||
|
||||
/// @brief Setup the DC motor
|
||||
/// @param pinIn1 the pin number for the in1 signal
|
||||
/// @param pinIn2 the pin number for the in2 signal
|
||||
/// @param direction the forward turning direction of the motor
|
||||
DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
|
||||
void SetMaxRPM(unsigned int rpm);
|
||||
|
||||
virtual void SetAngularVelocity(Spherical velocity) override;
|
||||
|
||||
bool reverse = false;
|
||||
|
||||
protected:
|
||||
unsigned char pinIn1 = 255;
|
||||
unsigned char pinIn2 = 255;
|
||||
unsigned int maxRpm = 200;
|
||||
};
|
||||
|
||||
class DRV8833 : public Thing {
|
||||
public:
|
||||
/// @brief Setup a DRV8833 motor controller
|
||||
/// @param pinAIn1 The pin number connected to the AIn1 port
|
||||
/// @param pinAIn2 The pin number connected to the AIn2 port
|
||||
/// @param pinBIn1 The pin number connected to the BIn1 port
|
||||
/// @param pinBIn2 The pin number connceted to the BIn2 port
|
||||
/// @param pinStandby The pin number connected to the standby port, 255
|
||||
/// indicated that the port is not connected
|
||||
/// @param reverseA The forward turning direction of motor A
|
||||
/// @param reverseB The forward turning direction of motor B
|
||||
DRV8833(Participant* participant,
|
||||
unsigned char pinAIn1,
|
||||
unsigned char pinAIn2,
|
||||
unsigned char pinBIn1,
|
||||
unsigned char pinBIn2,
|
||||
unsigned char pinStandby = 255,
|
||||
bool reverseA = false,
|
||||
bool reverseB = false);
|
||||
|
||||
DRV8833Motor* motorA = nullptr;
|
||||
DRV8833Motor* motorB = nullptr;
|
||||
|
||||
protected:
|
||||
unsigned char pinStandby = 255;
|
||||
};
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,23 +0,0 @@
|
||||
#include "DigitalInput.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
|
||||
: TouchSensor(participant) {
|
||||
this->pin = pin;
|
||||
|
||||
pinMode(pin, INPUT);
|
||||
}
|
||||
|
||||
void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
|
||||
this->touchedSomething = digitalRead(pin) == LOW;
|
||||
// std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
|
||||
// this->touchedSomething << "\n";
|
||||
Thing::Update(currentTimeMs, recursive);
|
||||
}
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,26 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Things/TouchSensor.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
/// @brief A digital input represents the stat of a digital GPIO pin
|
||||
class DigitalInput : public TouchSensor {
|
||||
public:
|
||||
/// @brief Create a new digital input
|
||||
/// @param participant The participant to use
|
||||
/// @param pin The digital pin
|
||||
DigitalInput(Participant* participant, unsigned char pin);
|
||||
|
||||
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
|
||||
virtual void Update(unsigned long currentTimeMs,
|
||||
bool recursive = false) override;
|
||||
|
||||
protected:
|
||||
/// @brief The pin used for digital input
|
||||
unsigned char pin = 0;
|
||||
};
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,68 +0,0 @@
|
||||
#include "UltrasonicSensor.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
UltrasonicSensor::UltrasonicSensor(Participant* participant,
|
||||
unsigned char pinTrigger,
|
||||
unsigned char pinEcho)
|
||||
: TouchSensor(participant) {
|
||||
this->pinTrigger = pinTrigger;
|
||||
this->pinEcho = pinEcho;
|
||||
|
||||
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
|
||||
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
|
||||
}
|
||||
|
||||
float UltrasonicSensor::GetDistance() {
|
||||
// Start the ultrasonic 'ping'
|
||||
digitalWrite(pinTrigger, LOW);
|
||||
delayMicroseconds(5);
|
||||
digitalWrite(pinTrigger, HIGH);
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(pinTrigger, LOW);
|
||||
|
||||
// Measure the duration of the pulse on the echo pin
|
||||
float duration_us =
|
||||
pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds
|
||||
|
||||
// Calculate the distance:
|
||||
// * Duration should be divided by 2, because the ping goes to the object
|
||||
// and back again. The distance to the object is therefore half the duration
|
||||
// of the pulse: duration_us /= 2;
|
||||
// * Then we need to convert from microseconds to seconds: duration_sec =
|
||||
// duration_us / 1000000;
|
||||
// * Now we calculate the distance based on the speed of sound (340 m/s):
|
||||
// distance = duration_sec * 340;
|
||||
// * The result calculation is therefore:
|
||||
this->distance = duration_us / 2 / 1000000 * 340;
|
||||
|
||||
// Filter faulty measurements. The sensor can often give values > 30 m which
|
||||
// are not correct
|
||||
// if (distance > 30)
|
||||
// distance = 0;
|
||||
|
||||
this->touchedSomething |= (this->distance <= this->touchDistance);
|
||||
|
||||
// std::cout << "Ultrasonic " << this->distance << " " <<
|
||||
// this->touchedSomething << "\n";
|
||||
|
||||
return distance;
|
||||
}
|
||||
|
||||
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
|
||||
this->touchedSomething = false;
|
||||
GetDistance();
|
||||
Thing::Update(currentTimeMs, recursive);
|
||||
}
|
||||
|
||||
// void UltrasonicSensor::ProcessBinary(char* bytes) {
|
||||
// this->touchedSomething = (bytes[0] == 1);
|
||||
// if (this->touchedSomething)
|
||||
// std::cout << "Touching something!\n";
|
||||
// }
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,45 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Things/TouchSensor.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
namespace Arduino {
|
||||
|
||||
/// @brief An HC-SR04 ultrasonic distance sensor
|
||||
class UltrasonicSensor : public TouchSensor {
|
||||
public:
|
||||
/// @brief Setup an ultrasonic sensor
|
||||
/// @param participant The participant to use
|
||||
/// @param pinTrigger The pin number of the trigger signal
|
||||
/// @param pinEcho The pin number of the echo signal
|
||||
UltrasonicSensor(Participant* participant,
|
||||
unsigned char pinTrigger,
|
||||
unsigned char pinEcho);
|
||||
|
||||
// parameters
|
||||
|
||||
/// @brief The distance at which the object is considered to be touched
|
||||
float touchDistance = 0.2f;
|
||||
|
||||
// state
|
||||
|
||||
/// @brief The last read distance
|
||||
float distance = 0;
|
||||
/// @brief erform an ultrasonic 'ping' to determine the distance to the
|
||||
/// nearest object
|
||||
/// @return the measured distance in meters to the nearest object
|
||||
float GetDistance();
|
||||
|
||||
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
|
||||
virtual void Update(unsigned long currentTimeMs,
|
||||
bool recursive = false) override;
|
||||
|
||||
protected:
|
||||
/// @brief The pin number of the trigger signal
|
||||
unsigned char pinTrigger = 0;
|
||||
/// @brief The pin number of the echo signal
|
||||
unsigned char pinEcho = 0;
|
||||
};
|
||||
|
||||
} // namespace Arduino
|
||||
} // namespace RoboidControl
|
||||
@ -1,58 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.13) # CMake version check
|
||||
if(ESP_PLATFORM)
|
||||
idf_component_register(
|
||||
SRC_DIRS "."
|
||||
INCLUDE_DIRS "."
|
||||
)
|
||||
else()
|
||||
project(RoboidControl)
|
||||
add_subdirectory(LinearAlgebra)
|
||||
add_subdirectory(Examples)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
set(sourcedirs
|
||||
.
|
||||
VectorAlgebra/src
|
||||
)
|
||||
|
||||
add_compile_definitions(GTEST)
|
||||
include(FetchContent)
|
||||
FetchContent_Declare(
|
||||
googletest
|
||||
DOWNLOAD_EXTRACT_TIMESTAMP ON
|
||||
URL https://github.com/google/googletest/archive/refs/heads/main.zip
|
||||
)
|
||||
|
||||
# For Windows: Prevent overriding the parent project's compiler/linker settings
|
||||
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
|
||||
FetchContent_MakeAvailable(googletest)
|
||||
|
||||
include_directories(
|
||||
.
|
||||
LinearAlgebra
|
||||
)
|
||||
file(GLOB srcs
|
||||
*.cpp
|
||||
Things/*.cpp
|
||||
Messages/*.cpp
|
||||
Arduino/*.cpp
|
||||
Posix/*.cpp
|
||||
Windows/*.cpp
|
||||
)
|
||||
add_library(RoboidControl STATIC ${srcs})
|
||||
|
||||
enable_testing()
|
||||
|
||||
file(GLOB_RECURSE test_srcs test/*_test.cc)
|
||||
add_executable(
|
||||
RoboidControlTest
|
||||
${test_srcs}
|
||||
)
|
||||
target_link_libraries(
|
||||
RoboidControlTest
|
||||
gtest_main
|
||||
RoboidControl
|
||||
LinearAlgebra
|
||||
)
|
||||
|
||||
include(GoogleTest)
|
||||
gtest_discover_tests(RoboidControlTest)
|
||||
endif()
|
||||
set(includedirs
|
||||
.
|
||||
VectorAlgebra/include
|
||||
)
|
||||
|
||||
idf_component_register(
|
||||
SRC_DIRS ${sourcedirs}
|
||||
INCLUDE_DIRS ${includedirs}
|
||||
REQUIRES arduino
|
||||
)
|
||||
|
||||
44
ControlledMotor.cpp
Normal file
44
ControlledMotor.cpp
Normal file
@ -0,0 +1,44 @@
|
||||
#include "ControlledMotor.h"
|
||||
#include <Arduino.h>
|
||||
ControlledMotor::ControlledMotor() {
|
||||
this->type = Thing::ControlledMotorType;
|
||||
}
|
||||
|
||||
ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
|
||||
: ControlledMotor() {
|
||||
this->motor = motor;
|
||||
this->encoder = encoder;
|
||||
}
|
||||
|
||||
void ControlledMotor::SetTargetSpeed(float velocity) {
|
||||
this->targetVelocity = velocity;
|
||||
this->rotationDirection =
|
||||
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
|
||||
}
|
||||
|
||||
void ControlledMotor::Update(float currentTimeMs) {
|
||||
actualVelocity =
|
||||
(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
|
||||
float error = targetVelocity - velocity;
|
||||
|
||||
float timeStep = currentTimeMs - lastUpdateTime;
|
||||
float acceleration =
|
||||
error * timeStep * pidP; // Just P is used at this moment
|
||||
motor->SetSpeed(targetVelocity + acceleration); // or something like that
|
||||
this->lastUpdateTime = currentTimeMs;
|
||||
}
|
||||
|
||||
float ControlledMotor::GetActualSpeed() {
|
||||
return actualVelocity;
|
||||
}
|
||||
|
||||
bool ControlledMotor::Drive(float distance) {
|
||||
if (!driving) {
|
||||
targetDistance = distance;
|
||||
startDistance = encoder->GetDistance();
|
||||
driving = true;
|
||||
}
|
||||
float totalDistance = encoder->GetDistance() - startDistance;
|
||||
bool completed = totalDistance > targetDistance;
|
||||
return completed;
|
||||
}
|
||||
59
ControlledMotor.h
Normal file
59
ControlledMotor.h
Normal file
@ -0,0 +1,59 @@
|
||||
#pragma once
|
||||
|
||||
#include "Encoder.h"
|
||||
#include "Motor.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A motor with speed control
|
||||
/// It uses a feedback loop from an encoder to regulate the speed
|
||||
/// The speed is measured in revolutions per second.
|
||||
class ControlledMotor : public Thing {
|
||||
public:
|
||||
ControlledMotor();
|
||||
ControlledMotor(Motor* motor, Encoder* encoder);
|
||||
|
||||
inline static bool CheckType(Thing* thing) {
|
||||
return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
|
||||
}
|
||||
float velocity;
|
||||
|
||||
float pidP = 1;
|
||||
float pidD = 0;
|
||||
float pidI = 0;
|
||||
|
||||
void Update(float currentTimeMs);
|
||||
|
||||
/// @brief Set the target speed for the motor controller
|
||||
/// @param speed the target in revolutions per second.
|
||||
void SetTargetSpeed(float speed);
|
||||
|
||||
/// @brief Get the actual speed from the encoder
|
||||
/// @return The speed in revolutions per second
|
||||
float GetActualSpeed();
|
||||
|
||||
bool Drive(float distance);
|
||||
|
||||
Motor* motor;
|
||||
Encoder* encoder;
|
||||
|
||||
protected:
|
||||
float lastUpdateTime;
|
||||
float targetVelocity;
|
||||
float actualVelocity;
|
||||
float netDistance = 0;
|
||||
float startDistance = 0;
|
||||
|
||||
enum Direction { Forward = 1, Reverse = -1 };
|
||||
|
||||
Direction rotationDirection;
|
||||
|
||||
bool driving = false;
|
||||
float targetDistance = 0;
|
||||
float lastEncoderPosition = 0;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
27
DistanceSensor.cpp
Normal file
27
DistanceSensor.cpp
Normal file
@ -0,0 +1,27 @@
|
||||
#include "DistanceSensor.h"
|
||||
|
||||
DistanceSensor::DistanceSensor() {
|
||||
this->type = Thing::DistanceSensorType;
|
||||
}
|
||||
|
||||
DistanceSensor::DistanceSensor(float triggerDistance) {
|
||||
this->triggerDistance = triggerDistance;
|
||||
}
|
||||
|
||||
float DistanceSensor::GetDistance() {
|
||||
return distance;
|
||||
};
|
||||
|
||||
void DistanceSensor::SetDistance(float distance) {
|
||||
this->distance = distance;
|
||||
}; // for simulation purposes
|
||||
|
||||
bool DistanceSensor::IsOn() {
|
||||
bool isOn = GetDistance() <= triggerDistance;
|
||||
return isOn;
|
||||
}
|
||||
|
||||
bool DistanceSensor::isOff() {
|
||||
bool isOff = GetDistance() > triggerDistance;
|
||||
return isOff;
|
||||
}
|
||||
31
DistanceSensor.h
Normal file
31
DistanceSensor.h
Normal file
@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
|
||||
#include "Sensor.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A Sensor which can measure the distance to the nearest object
|
||||
class DistanceSensor : public Sensor {
|
||||
public:
|
||||
DistanceSensor();
|
||||
DistanceSensor(float triggerDistance);
|
||||
/// @brief Determine the distance to the nearest object
|
||||
/// @return the measured distance in meters to the nearest object
|
||||
virtual float GetDistance();
|
||||
void SetDistance(float distance);
|
||||
|
||||
/// @brief The distance at which ObjectNearby triggers
|
||||
float triggerDistance = 1;
|
||||
|
||||
bool IsOn();
|
||||
|
||||
bool isOff();
|
||||
|
||||
protected:
|
||||
float distance = 0;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
2861
DoxyGen/Doxyfile
2861
DoxyGen/Doxyfile
File diff suppressed because it is too large
Load Diff
@ -1,226 +0,0 @@
|
||||
<doxygenlayout version="1.0">
|
||||
<!-- Generated by doxygen 1.8.18 -->
|
||||
<!-- Navigation index tabs for HTML output -->
|
||||
<navindex>
|
||||
<tab type="mainpage" visible="yes" title=""/>
|
||||
<tab type="pages" visible="yes" title="" intro=""/>
|
||||
<tab type="modules" visible="yes" title="" intro=""/>
|
||||
<tab type="namespaces" visible="yes" title="">
|
||||
<tab type="namespacelist" visible="yes" title="" intro=""/>
|
||||
<tab type="namespacemembers" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="interfaces" visible="yes" title="">
|
||||
<tab type="interfacelist" visible="yes" title="" intro=""/>
|
||||
<tab type="interfaceindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
<tab type="interfacehierarchy" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="classes" visible="yes" title="">
|
||||
<tab type="classlist" visible="yes" title="" intro=""/>
|
||||
<tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
<tab type="hierarchy" visible="yes" title="" intro=""/>
|
||||
<tab type="classmembers" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="structs" visible="yes" title="">
|
||||
<tab type="structlist" visible="yes" title="" intro=""/>
|
||||
<tab type="structindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
</tab>
|
||||
<tab type="exceptions" visible="yes" title="">
|
||||
<tab type="exceptionlist" visible="yes" title="" intro=""/>
|
||||
<tab type="exceptionindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
<tab type="exceptionhierarchy" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="files" visible="yes" title="">
|
||||
<tab type="filelist" visible="yes" title="" intro=""/>
|
||||
<tab type="globals" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="examples" visible="yes" title="" intro=""/>
|
||||
</navindex>
|
||||
|
||||
<!-- Layout definition for a class page -->
|
||||
<class>
|
||||
<briefdescription visible="no"/>
|
||||
<detaileddescription title=""/>
|
||||
<includes visible="$SHOW_INCLUDE_FILES"/>
|
||||
<inheritancegraph visible="$CLASS_GRAPH"/>
|
||||
<collaborationgraph visible="$COLLABORATION_GRAPH"/>
|
||||
<memberdecl>
|
||||
<nestedclasses visible="yes" title=""/>
|
||||
<publictypes title=""/>
|
||||
<services title=""/>
|
||||
<interfaces title=""/>
|
||||
<publicslots title=""/>
|
||||
<signals title=""/>
|
||||
<publicmethods title=""/>
|
||||
<publicstaticmethods title=""/>
|
||||
<publicattributes title=""/>
|
||||
<publicstaticattributes title=""/>
|
||||
<protectedtypes title=""/>
|
||||
<protectedslots title=""/>
|
||||
<protectedmethods title=""/>
|
||||
<protectedstaticmethods title=""/>
|
||||
<protectedattributes title=""/>
|
||||
<protectedstaticattributes title=""/>
|
||||
<packagetypes title=""/>
|
||||
<packagemethods title=""/>
|
||||
<packagestaticmethods title=""/>
|
||||
<packageattributes title=""/>
|
||||
<packagestaticattributes title=""/>
|
||||
<properties title=""/>
|
||||
<events title=""/>
|
||||
<privatetypes title=""/>
|
||||
<privateslots title=""/>
|
||||
<privatemethods title=""/>
|
||||
<privatestaticmethods title=""/>
|
||||
<privateattributes title=""/>
|
||||
<privatestaticattributes title=""/>
|
||||
<friends title=""/>
|
||||
<related title="" subtitle=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<memberdef>
|
||||
<inlineclasses title=""/>
|
||||
<typedefs title=""/>
|
||||
<enums title=""/>
|
||||
<services title=""/>
|
||||
<interfaces title=""/>
|
||||
<constructors title=""/>
|
||||
<functions title=""/>
|
||||
<related title=""/>
|
||||
<variables title=""/>
|
||||
<properties title=""/>
|
||||
<events title=""/>
|
||||
</memberdef>
|
||||
<allmemberslink visible="yes"/>
|
||||
<usedfiles visible="$SHOW_USED_FILES"/>
|
||||
<authorsection visible="yes"/>
|
||||
</class>
|
||||
|
||||
<!-- Layout definition for a namespace page -->
|
||||
<namespace>
|
||||
<briefdescription visible="yes"/>
|
||||
<memberdecl>
|
||||
<nestednamespaces visible="yes" title=""/>
|
||||
<constantgroups visible="yes" title=""/>
|
||||
<interfaces visible="yes" title=""/>
|
||||
<classes visible="yes" title=""/>
|
||||
<structs visible="yes" title=""/>
|
||||
<exceptions visible="yes" title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
<memberdef>
|
||||
<inlineclasses title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
</memberdef>
|
||||
<authorsection visible="yes"/>
|
||||
</namespace>
|
||||
|
||||
<!-- Layout definition for a file page -->
|
||||
<file>
|
||||
<briefdescription visible="yes"/>
|
||||
<includes visible="$SHOW_INCLUDE_FILES"/>
|
||||
<includegraph visible="$INCLUDE_GRAPH"/>
|
||||
<includedbygraph visible="$INCLUDED_BY_GRAPH"/>
|
||||
<sourcelink visible="yes"/>
|
||||
<memberdecl>
|
||||
<interfaces visible="yes" title=""/>
|
||||
<classes visible="yes" title=""/>
|
||||
<structs visible="yes" title=""/>
|
||||
<exceptions visible="yes" title=""/>
|
||||
<namespaces visible="yes" title=""/>
|
||||
<constantgroups visible="yes" title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
<memberdef>
|
||||
<inlineclasses title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
</memberdef>
|
||||
<authorsection/>
|
||||
</file>
|
||||
|
||||
<!-- Layout definition for a group page -->
|
||||
<group>
|
||||
<briefdescription visible="yes"/>
|
||||
<groupgraph visible="$GROUP_GRAPHS"/>
|
||||
<memberdecl>
|
||||
<nestedgroups visible="yes" title=""/>
|
||||
<dirs visible="yes" title=""/>
|
||||
<files visible="yes" title=""/>
|
||||
<namespaces visible="yes" title=""/>
|
||||
<classes visible="yes" title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<enumvalues title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<signals title=""/>
|
||||
<publicslots title=""/>
|
||||
<protectedslots title=""/>
|
||||
<privateslots title=""/>
|
||||
<events title=""/>
|
||||
<properties title=""/>
|
||||
<friends title=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
<memberdef>
|
||||
<pagedocs/>
|
||||
<inlineclasses title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<enumvalues title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<signals title=""/>
|
||||
<publicslots title=""/>
|
||||
<protectedslots title=""/>
|
||||
<privateslots title=""/>
|
||||
<events title=""/>
|
||||
<properties title=""/>
|
||||
<friends title=""/>
|
||||
</memberdef>
|
||||
<authorsection visible="yes"/>
|
||||
</group>
|
||||
|
||||
<!-- Layout definition for a directory page -->
|
||||
<directory>
|
||||
<briefdescription visible="yes"/>
|
||||
<directorygraph visible="yes"/>
|
||||
<memberdecl>
|
||||
<dirs visible="yes"/>
|
||||
<files visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
</directory>
|
||||
</doxygenlayout>
|
||||
28
Encoder.cpp
Normal file
28
Encoder.cpp
Normal file
@ -0,0 +1,28 @@
|
||||
#include "Encoder.h"
|
||||
|
||||
Encoder::Encoder(unsigned char transitionsPerRotation,
|
||||
unsigned char distancePerRotation) {
|
||||
//: Encoder::Encoder() {
|
||||
this->transitionsPerRevolution = transitionsPerRotation;
|
||||
this->distancePerRevolution = distancePerRotation;
|
||||
}
|
||||
|
||||
int Encoder::GetPulseCount() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
float Encoder::GetDistance() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
float Encoder::GetPulsesPerSecond(float currentTimeMs) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
float Encoder::GetRevolutionsPerSecond(float currentTimeMs) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
float Encoder::GetSpeed(float currentTimeMs) {
|
||||
return 0;
|
||||
}
|
||||
51
Encoder.h
Normal file
51
Encoder.h
Normal file
@ -0,0 +1,51 @@
|
||||
#pragma once
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief An Encoder measures the rotations of an axle using a rotary sensor
|
||||
/// Some encoders are able to detect direction, while others can not.
|
||||
class Encoder {
|
||||
public:
|
||||
/// @brief Creates a sensor which measures distance from pulses
|
||||
/// @param transitionsPerRevolution The number of pulse edges which make a
|
||||
/// full rotation
|
||||
/// @param distancePerRevolution The distance a wheel travels per full
|
||||
/// rotation
|
||||
Encoder(unsigned char transitionsPerRevolution = 1,
|
||||
unsigned char distancePerRevolution = 1);
|
||||
|
||||
/// @brief Get the total number of pulses since the previous call
|
||||
/// @return The number of pulses, is zero or greater
|
||||
virtual int GetPulseCount();
|
||||
/// @brief Get the pulse speed since the previous call
|
||||
/// @param currentTimeMs The clock time in milliseconds
|
||||
/// @return The average pulses per second in the last period.
|
||||
virtual float GetPulsesPerSecond(float currentTimeMs);
|
||||
|
||||
/// @brief Get the distance traveled since the previous call
|
||||
/// @return The distance in meters.
|
||||
virtual float GetDistance();
|
||||
|
||||
/// @brief Get the rotation speed since the previous call
|
||||
/// @param currentTimeMs The clock time in milliseconds
|
||||
/// @return The speed in rotations per second
|
||||
virtual float GetRevolutionsPerSecond(float currentTimeMs);
|
||||
|
||||
/// @brief Get the speed since the previous call
|
||||
/// @param currentTimeMs The clock time in milliseconds
|
||||
/// @return The speed in meters per second.
|
||||
/// @note this value is dependent on the accurate setting of the
|
||||
/// transitionsPerRevolution and distancePerRevolution parameters;
|
||||
virtual float GetSpeed(float currentTimeMs);
|
||||
|
||||
/// @brief The numer of pulses corresponding to a full rotation of the axle
|
||||
unsigned char transitionsPerRevolution = 1;
|
||||
/// @brief The number of revolutions which makes the wheel move forward 1
|
||||
/// meter
|
||||
unsigned char distancePerRevolution = 1;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
@ -1,50 +0,0 @@
|
||||
#include "Thing.h"
|
||||
#include "Things/DifferentialDrive.h"
|
||||
#include "Things/TouchSensor.h"
|
||||
|
||||
#if defined(ARDUINO)
|
||||
#include "Arduino.h"
|
||||
|
||||
#else
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
using namespace std::this_thread;
|
||||
using namespace std::chrono;
|
||||
#endif
|
||||
|
||||
using namespace RoboidControl;
|
||||
|
||||
int main() {
|
||||
// The robot's propulsion is a differential drive
|
||||
DifferentialDrive* bb2b = new DifferentialDrive();
|
||||
// Is has a touch sensor at the front left of the roboid
|
||||
TouchSensor* touchLeft = new TouchSensor(bb2b);
|
||||
// and other one on the right
|
||||
TouchSensor* touchRight = new TouchSensor(bb2b);
|
||||
|
||||
// Do forever:
|
||||
while (true) {
|
||||
// The left wheel turns forward when nothing is touched on the right side
|
||||
// and turn backward when the roboid hits something on the right
|
||||
float leftWheelSpeed = (touchRight->touchedSomething) ? -600.0f : 600.0f;
|
||||
// The right wheel does the same, but instead is controlled by
|
||||
// touches on the left side
|
||||
float rightWheelSpeed = (touchLeft->touchedSomething) ? -600.0f : 600.0f;
|
||||
// When both sides are touching something, both wheels will turn backward
|
||||
// and the roboid will move backwards
|
||||
bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
|
||||
|
||||
// Update the roboid state
|
||||
bb2b->Update(true);
|
||||
|
||||
// and sleep for 100ms
|
||||
#if defined(ARDUINO)
|
||||
delay(100);
|
||||
#else
|
||||
sleep_for(milliseconds(100));
|
||||
#endif
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,25 +0,0 @@
|
||||
# examples/CMakeLists.txt
|
||||
|
||||
# Specify the minimum CMake version
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
|
||||
# Specify the path to the main project directory
|
||||
set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
|
||||
|
||||
# Set the project name
|
||||
project(Examples)
|
||||
|
||||
include_directories(..)
|
||||
|
||||
# Add the executable for the main project
|
||||
#add_executable(MainExecutable ${SOURCES})
|
||||
# Find the main project library (assuming it's defined in the root CMakeLists.txt)
|
||||
#find_package(RoboidControl REQUIRED) # Replace MyLibrary with your actual library name
|
||||
|
||||
# Add example executables
|
||||
add_executable(BB2B BB2B.cpp)
|
||||
target_link_libraries(
|
||||
BB2B
|
||||
RoboidControl
|
||||
LinearAlgebra
|
||||
)
|
||||
2
LinearAlgebra/.gitignore
vendored
2
LinearAlgebra/.gitignore
vendored
@ -1,2 +0,0 @@
|
||||
build
|
||||
.vscode
|
||||
@ -1,32 +0,0 @@
|
||||
# You can override the included template(s) by including variable overrides
|
||||
# SAST customization: https://docs.gitlab.com/ee/user/application_security/sast/#customizing-the-sast-settings
|
||||
# Secret Detection customization: https://docs.gitlab.com/ee/user/application_security/secret_detection/#customizing-settings
|
||||
# Dependency Scanning customization: https://docs.gitlab.com/ee/user/application_security/dependency_scanning/#customizing-the-dependency-scanning-settings
|
||||
# Note that environment variables can be set in several places
|
||||
# See https://docs.gitlab.com/ee/ci/variables/#cicd-variable-precedence
|
||||
#
|
||||
# Specify the docker image to use (only used if using docker runners)
|
||||
# See http://doc.gitlab.com/ee/ci/docker/using_docker_images.html)
|
||||
|
||||
default:
|
||||
image: rikorose/gcc-cmake
|
||||
stages:
|
||||
- test
|
||||
unit-test-job:
|
||||
stage: test
|
||||
script:
|
||||
- mkdir build
|
||||
- cd build
|
||||
- cmake ..
|
||||
- cmake --build .
|
||||
- export GTEST_OUTPUT="xml:report.xml"
|
||||
- ls -la
|
||||
- "./LinearAlgebraTest"
|
||||
artifacts:
|
||||
when: always
|
||||
reports:
|
||||
junit: build/report.xml
|
||||
sast:
|
||||
stage: test
|
||||
include:
|
||||
- template: Security/SAST.gitlab-ci.yml
|
||||
@ -1,394 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "Angle.h"
|
||||
#include <math.h>
|
||||
#include "FloatSingle.h"
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
//===== AngleSingle, AngleOf<float>
|
||||
|
||||
template <>
|
||||
AngleOf<float> AngleOf<float>::Degrees(float degrees) {
|
||||
if (isfinite(degrees)) {
|
||||
while (degrees < -180)
|
||||
degrees += 360;
|
||||
while (degrees >= 180)
|
||||
degrees -= 360;
|
||||
}
|
||||
|
||||
return AngleOf<float>(degrees);
|
||||
}
|
||||
|
||||
template <>
|
||||
AngleOf<float> AngleOf<float>::Radians(float radians) {
|
||||
if (isfinite(radians)) {
|
||||
while (radians <= -pi)
|
||||
radians += 2 * pi;
|
||||
while (radians > pi)
|
||||
radians -= 2 * pi;
|
||||
}
|
||||
|
||||
return Binary(radians * Rad2Deg);
|
||||
}
|
||||
|
||||
template <>
|
||||
float AngleOf<float>::InDegrees() const {
|
||||
return this->value;
|
||||
}
|
||||
|
||||
template <>
|
||||
float AngleOf<float>::InRadians() const {
|
||||
return this->value * Deg2Rad;
|
||||
}
|
||||
|
||||
//===== Angle16, AngleOf<signed short>
|
||||
|
||||
template <>
|
||||
AngleOf<signed short> AngleOf<signed short>::Degrees(float degrees) {
|
||||
// map float [-180..180) to integer [-32768..32767]
|
||||
signed short value = (signed short)roundf(degrees / 360.0F * 65536.0F);
|
||||
return Binary(value);
|
||||
}
|
||||
|
||||
template <>
|
||||
AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
|
||||
if (!isfinite(radians))
|
||||
return AngleOf<signed short>::zero;
|
||||
|
||||
// map float [-PI..PI) to integer [-32768..32767]
|
||||
signed short value = (signed short)roundf(radians / pi * 32768.0F);
|
||||
return Binary(value);
|
||||
}
|
||||
|
||||
template <>
|
||||
float AngleOf<signed short>::InDegrees() const {
|
||||
float degrees = this->value / 65536.0f * 360.0f;
|
||||
return degrees;
|
||||
}
|
||||
|
||||
template <>
|
||||
float AngleOf<signed short>::InRadians() const {
|
||||
float radians = this->value / 65536.0f * (2 * pi);
|
||||
return radians;
|
||||
}
|
||||
|
||||
//===== Angle8, AngleOf<signed char>
|
||||
|
||||
template <>
|
||||
AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
|
||||
// map float [-180..180) to integer [-128..127)
|
||||
signed char value = (signed char)roundf(degrees / 360.0F * 256.0F);
|
||||
return Binary(value);
|
||||
}
|
||||
|
||||
template <>
|
||||
AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
|
||||
if (!isfinite(radians))
|
||||
return AngleOf<signed char>::zero;
|
||||
|
||||
// map float [-pi..pi) to integer [-128..127)
|
||||
signed char value = (signed char)roundf(radians / pi * 128.0f);
|
||||
return Binary(value);
|
||||
}
|
||||
|
||||
template <>
|
||||
float AngleOf<signed char>::InDegrees() const {
|
||||
float degrees = this->value / 256.0f * 360.0f;
|
||||
return degrees;
|
||||
}
|
||||
|
||||
template <>
|
||||
float AngleOf<signed char>::InRadians() const {
|
||||
float radians = this->value / 128.0f * pi;
|
||||
return radians;
|
||||
}
|
||||
|
||||
//===== Generic
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T>::AngleOf() : value(0) {}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
|
||||
|
||||
template <typename T>
|
||||
const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Binary(T rawValue) {
|
||||
AngleOf<T> angle = AngleOf<T>();
|
||||
angle.SetBinary(rawValue);
|
||||
return angle;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T AngleOf<T>::GetBinary() const {
|
||||
return this->value;
|
||||
}
|
||||
template <typename T>
|
||||
void AngleOf<T>::SetBinary(T rawValue) {
|
||||
this->value = rawValue;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
|
||||
return this->value == angle.value;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool AngleOf<T>::operator>(AngleOf<T> angle) const {
|
||||
return this->value > angle.value;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
|
||||
return this->value >= angle.value;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool AngleOf<T>::operator<(AngleOf<T> angle) const {
|
||||
return this->value < angle.value;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
|
||||
return this->value <= angle.value;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
signed int AngleOf<T>::Sign(AngleOf<T> angle) {
|
||||
if (angle.value < 0)
|
||||
return -1;
|
||||
if (angle.value > 0)
|
||||
return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Abs(AngleOf<T> angle) {
|
||||
if (Sign(angle) < 0)
|
||||
return -angle;
|
||||
else
|
||||
return angle;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::operator-() const {
|
||||
AngleOf<T> angle = Binary(-this->value);
|
||||
return angle;
|
||||
}
|
||||
|
||||
template <>
|
||||
AngleOf<float> AngleOf<float>::operator-(const AngleOf<float>& angle) const {
|
||||
AngleOf<float> r = Binary(this->value - angle.value);
|
||||
r = Normalize(r);
|
||||
return r;
|
||||
}
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::operator-(const AngleOf<T>& angle) const {
|
||||
AngleOf<T> r = Binary(this->value - angle.value);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <>
|
||||
AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& angle) const {
|
||||
AngleOf<float> r = Binary(this->value + angle.value);
|
||||
r = Normalize(r);
|
||||
return r;
|
||||
}
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::operator+(const AngleOf<T>& angle) const {
|
||||
AngleOf<T> r = Binary(this->value + angle.value);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <>
|
||||
AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float>& angle) {
|
||||
this->value += angle.value;
|
||||
this->Normalize();
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
|
||||
this->value += angle.value;
|
||||
return *this;
|
||||
}
|
||||
|
||||
// This defintion is not matching the declaration in the header file somehow
|
||||
// template <typename T>
|
||||
// AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
|
||||
// return AngleOf::Degrees((float)angle.InDegrees() * factor);
|
||||
// }
|
||||
|
||||
// This defintion is not matching the declaration in the header file somehow
|
||||
// template <typename T>
|
||||
// AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
|
||||
// return AngleOf::Degrees((float)factor * angle.InDegrees());
|
||||
// }
|
||||
|
||||
template <typename T>
|
||||
void AngleOf<T>::Normalize() {
|
||||
float angleValue = this->InDegrees();
|
||||
if (!isfinite(angleValue))
|
||||
return;
|
||||
|
||||
while (angleValue <= -180)
|
||||
angleValue += 360;
|
||||
while (angleValue > 180)
|
||||
angleValue -= 360;
|
||||
*this = AngleOf::Degrees(angleValue);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
|
||||
float angleValue = angle.InDegrees();
|
||||
if (!isfinite(angleValue))
|
||||
return angle;
|
||||
|
||||
while (angleValue <= -180)
|
||||
angleValue += 360;
|
||||
while (angleValue > 180)
|
||||
angleValue -= 360;
|
||||
return AngleOf::Degrees(angleValue);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
|
||||
float r = Float::Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
|
||||
return AngleOf<T>::Degrees(r);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
|
||||
AngleOf<T> toAngle,
|
||||
float maxDegrees) {
|
||||
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
|
||||
AngleOf<T> d = toAngle - fromAngle;
|
||||
float dDegrees = Abs(d).InDegrees();
|
||||
d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
|
||||
if (Sign(d) < 0)
|
||||
d = -d;
|
||||
|
||||
return fromAngle + d;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
float AngleOf<T>::Cos(AngleOf<T> angle) {
|
||||
return cosf(angle.InRadians());
|
||||
}
|
||||
template <typename T>
|
||||
float AngleOf<T>::Sin(AngleOf<T> angle) {
|
||||
return sinf(angle.InRadians());
|
||||
}
|
||||
template <typename T>
|
||||
float AngleOf<T>::Tan(AngleOf<T> angle) {
|
||||
return tanf(angle.InRadians());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Acos(float f) {
|
||||
return AngleOf<T>::Radians(acosf(f));
|
||||
}
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Asin(float f) {
|
||||
return AngleOf<T>::Radians(asinf(f));
|
||||
}
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Atan(float f) {
|
||||
return AngleOf<T>::Radians(atanf(f));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::Atan2(float y, float x) {
|
||||
return AngleOf<T>::Radians(atan2f(y, x));
|
||||
}
|
||||
|
||||
// template <>
|
||||
// float AngleOf<float>::CosineRuleSide(float a, float b, AngleOf<float> gamma)
|
||||
// {
|
||||
// float a2 = a * a;
|
||||
// float b2 = b * b;
|
||||
// float d =
|
||||
// a2 + b2 -
|
||||
// 2 * a * b * Cos(gamma); // cosf(gamma *
|
||||
// Passer::LinearAlgebra::Deg2Rad);
|
||||
// // Catch edge cases where float inacuracies lead tot nans
|
||||
// if (d < 0)
|
||||
// return 0.0f;
|
||||
|
||||
// float c = sqrtf(d);
|
||||
// return c;
|
||||
// }
|
||||
|
||||
template <typename T>
|
||||
float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
|
||||
float a2 = a * a;
|
||||
float b2 = b * b;
|
||||
float d =
|
||||
a2 + b2 -
|
||||
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
|
||||
// Catch edge cases where float inacuracies lead tot nans
|
||||
if (d < 0)
|
||||
return 0;
|
||||
|
||||
float c = sqrtf(d);
|
||||
return c;
|
||||
}
|
||||
|
||||
// template <>
|
||||
// AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
|
||||
// float a2 = a * a;
|
||||
// float b2 = b * b;
|
||||
// float c2 = c * c;
|
||||
// float d = (a2 + b2 - c2) / (2 * a * b);
|
||||
// // Catch edge cases where float inacuracies lead tot nans
|
||||
// if (d >= 1)
|
||||
// return 0.0f;
|
||||
// if (d <= -1)
|
||||
// return 180.0f;
|
||||
|
||||
// float gamma = acosf(d) * Rad2Deg;
|
||||
// return gamma;
|
||||
// }
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::CosineRuleAngle(float a, float b, float c) {
|
||||
float a2 = a * a;
|
||||
float b2 = b * b;
|
||||
float c2 = c * c;
|
||||
float d = (a2 + b2 - c2) / (2 * a * b);
|
||||
// Catch edge cases where float inacuracies lead tot nans
|
||||
if (d >= 1)
|
||||
return AngleOf<T>();
|
||||
if (d <= -1)
|
||||
return AngleOf<T>::Degrees(180);
|
||||
|
||||
// float gamma = acosf(d) * Rad2Deg;
|
||||
AngleOf<T> gamma = Acos(d);
|
||||
return gamma;
|
||||
}
|
||||
|
||||
// template <>
|
||||
// AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
|
||||
// AngleOf<float> beta,
|
||||
// float b) {
|
||||
// float deg2rad = Deg2Rad;
|
||||
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
|
||||
// return alpha;
|
||||
// }
|
||||
template <typename T>
|
||||
AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
|
||||
// float deg2rad = Deg2Rad;
|
||||
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
|
||||
AngleOf<T> alpha = Asin(a * Sin(beta) / b);
|
||||
return alpha;
|
||||
}
|
||||
|
||||
template class AngleOf<float>;
|
||||
template class AngleOf<signed char>;
|
||||
template class AngleOf<signed short>;
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
@ -1,227 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef ANGLE_H
|
||||
#define ANGLE_H
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
static float pi = 3.1415927410125732421875F;
|
||||
|
||||
static float Rad2Deg = 360.0f / (pi * 2);
|
||||
static float Deg2Rad = (pi * 2) / 360.0f;
|
||||
|
||||
/// @brief An angle in various representations.
|
||||
/// @tparam T The internal type used for the representation of the angle.
|
||||
/// The angle is internally limited to (-180..180] degrees or (-PI...PI]
|
||||
/// radians. When an angle exceeds this range, it is normalized to a value
|
||||
/// within the range.
|
||||
template <typename T>
|
||||
class AngleOf {
|
||||
public:
|
||||
/// @brief Create a new angle with a zero value
|
||||
AngleOf<T>();
|
||||
|
||||
/// @brief An zero value angle
|
||||
const static AngleOf<T> zero;
|
||||
// const static AngleOf<T> deg90;
|
||||
// const static AngleOf<T> deg180;
|
||||
|
||||
/// @brief Creates an angle in degrees
|
||||
/// @param degrees the angle in degrees
|
||||
/// @return The angle value
|
||||
static AngleOf<T> Degrees(float degrees);
|
||||
/// @brief Creates an angle in radians
|
||||
/// @param radians the angle in radians
|
||||
/// @return The angle value
|
||||
static AngleOf<T> Radians(float radians);
|
||||
|
||||
/// @brief Creates an angle from a raw value
|
||||
/// @param rawValue the raw value to use for the angle
|
||||
/// @return The the angle
|
||||
static AngleOf<T> Binary(T rawValue);
|
||||
|
||||
/// @brief Get the angle value in degrees
|
||||
/// @return The angle value in degrees
|
||||
float InDegrees() const;
|
||||
/// @brief Get the angle value in radians
|
||||
/// @return The angle value in radians
|
||||
float InRadians() const;
|
||||
|
||||
/// @brief Get the raw value for the angle
|
||||
/// @return The raw value
|
||||
T GetBinary() const;
|
||||
/// @brief Set the raw value of the angle
|
||||
/// @param rawValue The raw value
|
||||
void SetBinary(T rawValue);
|
||||
|
||||
/// @brief Tests whether this angle is equal to the given angle
|
||||
/// @param angle The angle to compare to
|
||||
/// @return True when the angles are equal, False otherwise
|
||||
/// @note The equality is determine within the limits of precision of the raw
|
||||
/// type T
|
||||
bool operator==(const AngleOf<T> angle) const;
|
||||
/// @brief Tests if this angle is greater than the given angle
|
||||
/// @param angle The given angle
|
||||
/// @return True when this angle is greater than the given angle, False
|
||||
/// otherwise
|
||||
bool operator>(AngleOf<T> angle) const;
|
||||
/// @brief Tests if this angle is greater than or equal to the given angle
|
||||
/// @param angle The given angle
|
||||
/// @return True when this angle is greater than or equal to the given angle.
|
||||
/// False otherwise.
|
||||
bool operator>=(AngleOf<T> angle) const;
|
||||
/// @brief Tests if this angle is less than the given angle
|
||||
/// @param angle The given angle
|
||||
/// @return True when this angle is less than the given angle. False
|
||||
/// otherwise.
|
||||
bool operator<(AngleOf<T> angle) const;
|
||||
/// @brief Tests if this angle is less than or equal to the given angle
|
||||
/// @param angle The given angle
|
||||
/// @return True when this angle is less than or equal to the given angle.
|
||||
/// False otherwise.
|
||||
bool operator<=(AngleOf<T> angle) const;
|
||||
|
||||
/// @brief Returns the sign of the angle
|
||||
/// @param angle The angle
|
||||
/// @return -1 when the angle is negative, 1 when it is positive and 0
|
||||
/// otherwise.
|
||||
static signed int Sign(AngleOf<T> angle);
|
||||
/// @brief Returns the magnitude of the angle
|
||||
/// @param angle The angle
|
||||
/// @return The positive magitude of the angle.
|
||||
/// Negative values are negated to get a positive result
|
||||
static AngleOf<T> Abs(AngleOf<T> angle);
|
||||
|
||||
/// @brief Negate the angle
|
||||
/// @return The negated angle
|
||||
AngleOf<T> operator-() const;
|
||||
/// @brief Substract another angle from this angle
|
||||
/// @param angle The angle to subtract from this angle
|
||||
/// @return The result of the subtraction
|
||||
AngleOf<T> operator-(const AngleOf<T>& angle) const;
|
||||
/// @brief Add another angle from this angle
|
||||
/// @param angle The angle to add to this angle
|
||||
/// @return The result of the addition
|
||||
AngleOf<T> operator+(const AngleOf<T>& angle) const;
|
||||
/// @brief Add another angle to this angle
|
||||
/// @param angle The angle to add to this angle
|
||||
/// @return The result of the addition
|
||||
AngleOf<T> operator+=(const AngleOf<T>& angle);
|
||||
|
||||
/// @brief Mutliplies the angle
|
||||
/// @param angle The angle to multiply
|
||||
/// @param factor The factor by which the angle is multiplied
|
||||
/// @return The multiplied angle
|
||||
friend AngleOf<T> operator*(const AngleOf<T>& angle, float factor) {
|
||||
return AngleOf::Degrees((float)angle.InDegrees() * factor);
|
||||
}
|
||||
/// @brief Multiplies the angle
|
||||
/// @param factor The factor by which the angle is multiplies
|
||||
/// @param angle The angle to multiply
|
||||
/// @return The multiplied angle
|
||||
friend AngleOf<T> operator*(float factor, const AngleOf<T>& angle) {
|
||||
return AngleOf::Degrees((float)factor * angle.InDegrees());
|
||||
}
|
||||
|
||||
/// @brief Normalizes the angle to (-180..180] or (-PI..PI]
|
||||
/// Should not be needed but available in case it is.
|
||||
void Normalize();
|
||||
/// @brief Normalizes the angle to (-180..180] or (-PI..PI]
|
||||
/// @param angle The angle to normalize
|
||||
/// @return The normalized angle;
|
||||
static AngleOf<T> Normalize(AngleOf<T> angle);
|
||||
/// @brief Clamps the angle value between the two given angles
|
||||
/// @param angle The angle to clamp
|
||||
/// @param min The minimum angle
|
||||
/// @param max The maximum angle
|
||||
/// @return The clamped value
|
||||
/// @remark When the min value is greater than the max value, angle is
|
||||
/// returned unclamped.
|
||||
static AngleOf<T> Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max);
|
||||
// static AngleOf<T> Difference(AngleOf<T> a, AngleOf<T> b) {
|
||||
// AngleOf<T> r = Normalize(b.InDegrees() - a.InDegrees());
|
||||
// return r;
|
||||
// };
|
||||
|
||||
/// @brief Rotates an angle towards another angle with a max distance
|
||||
/// @param fromAngle The angle to start from
|
||||
/// @param toAngle The angle to rotate towards
|
||||
/// @param maxAngle The maximum angle to rotate
|
||||
/// @return The rotated angle
|
||||
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
|
||||
AngleOf<T> toAngle,
|
||||
float maxAngle);
|
||||
|
||||
/// @brief Calculates the cosine of an angle
|
||||
/// @param angle The given angle
|
||||
/// @return The cosine of the angle
|
||||
static float Cos(AngleOf<T> angle);
|
||||
/// @brief Calculates the sine of an angle
|
||||
/// @param angle The given angle
|
||||
/// @return The sine of the angle
|
||||
static float Sin(AngleOf<T> angle);
|
||||
/// @brief Calculates the tangent of an angle
|
||||
/// @param angle The given angle
|
||||
/// @return The tangent of the angle
|
||||
static float Tan(AngleOf<T> angle);
|
||||
|
||||
/// @brief Calculates the arc cosine angle
|
||||
/// @param f The value
|
||||
/// @return The arc cosine for the given value
|
||||
static AngleOf<T> Acos(float f);
|
||||
/// @brief Calculates the arc sine angle
|
||||
/// @param f The value
|
||||
/// @return The arc sine for the given value
|
||||
static AngleOf<T> Asin(float f);
|
||||
/// @brief Calculates the arc tangent angle
|
||||
/// @param f The value
|
||||
/// @return The arc tangent for the given value
|
||||
static AngleOf<T> Atan(float f);
|
||||
/// @brief Calculates the tangent for the given values
|
||||
/// @param y The vertical value
|
||||
/// @param x The horizontal value
|
||||
/// @return The tanget for the given values
|
||||
/// Uses the y and x signs to compute the quadrant
|
||||
static AngleOf<T> Atan2(float y, float x);
|
||||
|
||||
/// @brief Computes the length of a side using the rule of cosines
|
||||
/// @param a The length of side A
|
||||
/// @param b The length of side B
|
||||
/// @param gamma The angle of the corner opposing side C
|
||||
/// @return The length of side C
|
||||
static float CosineRuleSide(float a, float b, AngleOf<T> gamma);
|
||||
/// @brief Computes the angle of a corner using the rule of cosines
|
||||
/// @param a The length of side A
|
||||
/// @param b The length of side B
|
||||
/// @param c The length of side C
|
||||
/// @return The angle of the corner opposing side C
|
||||
static AngleOf<T> CosineRuleAngle(float a, float b, float c);
|
||||
|
||||
/// @brief Computes the angle of a corner using the rule of sines
|
||||
/// @param a The length of side A
|
||||
/// @param beta the angle of the corner opposing side B
|
||||
/// @param c The length of side C
|
||||
/// @return The angle of the corner opposing side A
|
||||
static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);
|
||||
|
||||
private:
|
||||
T value;
|
||||
|
||||
AngleOf<T>(T rawValue);
|
||||
};
|
||||
|
||||
using AngleSingle = AngleOf<float>;
|
||||
using Angle16 = AngleOf<signed short>;
|
||||
using Angle8 = AngleOf<signed char>;
|
||||
|
||||
#if defined(ARDUINO)
|
||||
using Angle = Angle16;
|
||||
#else
|
||||
using Angle = AngleSingle;
|
||||
#endif
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
|
||||
#endif
|
||||
@ -1,65 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.13) # CMake version check
|
||||
if(ESP_PLATFORM)
|
||||
idf_component_register(
|
||||
SRC_DIRS "."
|
||||
INCLUDE_DIRS "."
|
||||
)
|
||||
else()
|
||||
project(LinearAlgebra)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
|
||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||
|
||||
add_compile_definitions(GTEST)
|
||||
include(FetchContent)
|
||||
FetchContent_Declare(
|
||||
googletest
|
||||
DOWNLOAD_EXTRACT_TIMESTAMP ON
|
||||
URL https://github.com/google/googletest/archive/refs/heads/main.zip
|
||||
)
|
||||
|
||||
# For Windows: Prevent overriding the parent project's compiler/linker settings
|
||||
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
|
||||
FetchContent_MakeAvailable(googletest)
|
||||
|
||||
include_directories(.)
|
||||
file(GLOB srcs
|
||||
*.cpp
|
||||
)
|
||||
add_library(LinearAlgebra STATIC ${srcs}
|
||||
# "FloatSingle.cpp"
|
||||
# "Angle.cpp"
|
||||
# "Vector2.cpp"
|
||||
# "Vector3.cpp"
|
||||
# "Quaternion.cpp"
|
||||
# "Polar.cpp"
|
||||
# "Spherical.cpp"
|
||||
# "Matrix.cpp"
|
||||
# "SwingTwist.cpp"
|
||||
# "Direction.cpp"
|
||||
)
|
||||
|
||||
enable_testing()
|
||||
|
||||
file(GLOB_RECURSE test_srcs test/*_test.cc)
|
||||
add_executable(
|
||||
LinearAlgebraTest
|
||||
${test_srcs}
|
||||
)
|
||||
target_link_libraries(
|
||||
LinearAlgebraTest
|
||||
gtest_main
|
||||
LinearAlgebra
|
||||
)
|
||||
|
||||
if(MSVC)
|
||||
target_compile_options(LinearAlgebraTest PRIVATE /W4 /WX)
|
||||
# else()
|
||||
# target_compile_options(LinearAlgebraTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
|
||||
endif()
|
||||
|
||||
|
||||
include(GoogleTest)
|
||||
gtest_discover_tests(LinearAlgebraTest)
|
||||
endif()
|
||||
|
||||
@ -1,102 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "Direction.h"
|
||||
|
||||
#include "Quaternion.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
template <typename T>
|
||||
DirectionOf<T>::DirectionOf() {
|
||||
this->horizontal = AngleOf<T>();
|
||||
this->vertical = AngleOf<T>();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
|
||||
this->horizontal = horizontal;
|
||||
this->vertical = vertical;
|
||||
Normalize();
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
const DirectionOf<T> DirectionOf<T>::forward =
|
||||
DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
|
||||
template <typename T>
|
||||
const DirectionOf<T> DirectionOf<T>::back =
|
||||
DirectionOf<T>(AngleOf<T>::Degrees(180), AngleOf<T>());
|
||||
template <typename T>
|
||||
const DirectionOf<T> DirectionOf<T>::up =
|
||||
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::Degrees(90));
|
||||
template <typename T>
|
||||
const DirectionOf<T> DirectionOf<T>::down =
|
||||
DirectionOf<T>(AngleOf<T>(), AngleOf<T>::Degrees(-90));
|
||||
template <typename T>
|
||||
const DirectionOf<T> DirectionOf<T>::left =
|
||||
DirectionOf<T>(AngleOf<T>::Degrees(-90), AngleOf<T>());
|
||||
template <typename T>
|
||||
const DirectionOf<T> DirectionOf<T>::right =
|
||||
DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
|
||||
|
||||
template <typename T>
|
||||
Vector3 DirectionOf<T>::ToVector3() const {
|
||||
Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
|
||||
this->horizontal.InDegrees(), 0);
|
||||
Vector3 v = q * Vector3::forward;
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
|
||||
DirectionOf<T> d;
|
||||
d.horizontal = AngleOf<T>::Atan2(
|
||||
vector.Right(),
|
||||
vector
|
||||
.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
|
||||
d.vertical =
|
||||
AngleOf<T>::Degrees(-90) -
|
||||
AngleOf<T>::Acos(
|
||||
vector.Up()); // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
|
||||
d.Normalize();
|
||||
return d;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
DirectionOf<T> DirectionOf<T>::Degrees(float horizontal, float vertical) {
|
||||
return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
|
||||
AngleOf<T>::Degrees(vertical));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
DirectionOf<T> DirectionOf<T>::Radians(float horizontal, float vertical) {
|
||||
return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
|
||||
AngleOf<T>::Radians(vertical));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool DirectionOf<T>::operator==(const DirectionOf<T> direction) const {
|
||||
return (this->horizontal == direction.horizontal) &&
|
||||
(this->vertical == direction.vertical);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
DirectionOf<T> DirectionOf<T>::operator-() const {
|
||||
DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
|
||||
-this->vertical);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void DirectionOf<T>::Normalize() {
|
||||
if (this->vertical > AngleOf<T>::Degrees(90) ||
|
||||
this->vertical < AngleOf<T>::Degrees(-90)) {
|
||||
this->horizontal += AngleOf<T>::Degrees(180);
|
||||
this->vertical = AngleOf<T>::Degrees(180) - this->vertical;
|
||||
}
|
||||
}
|
||||
|
||||
template class DirectionOf<float>;
|
||||
template class DirectionOf<signed short>;
|
||||
@ -1,104 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef DIRECTION_H
|
||||
#define DIRECTION_H
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
struct Vector3;
|
||||
|
||||
/// @brief A direction using angles in various representations
|
||||
/// @tparam T The implementation type used for the representation of the angles
|
||||
/// A direction is represented using two angles:
|
||||
/// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
|
||||
/// degrees which is a rotation in the horizontal plane
|
||||
/// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
|
||||
/// which is the rotation in the up/down direction applied after the horizontal
|
||||
/// rotation has been applied.
|
||||
/// The angles are automatically normalized to stay within the abovenmentioned
|
||||
/// ranges.
|
||||
template <typename T>
|
||||
class DirectionOf {
|
||||
public:
|
||||
/// @brief horizontal angle, range= (-180..180]
|
||||
AngleOf<T> horizontal;
|
||||
/// @brief vertical angle, range in degrees = (-90..90]
|
||||
AngleOf<T> vertical;
|
||||
|
||||
/// @brief Create a new direction with zero angles
|
||||
DirectionOf<T>();
|
||||
/// @brief Create a new direction
|
||||
/// @param horizontal The horizontal angle
|
||||
/// @param vertical The vertical angle.
|
||||
DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
|
||||
|
||||
/// @brief Convert the direction into a carthesian vector
|
||||
/// @return The carthesian vector corresponding to this direction.
|
||||
Vector3 ToVector3() const;
|
||||
/// @brief Convert a carthesian vector into a direction
|
||||
/// @param v The carthesian vector
|
||||
/// @return The direction.
|
||||
/// @note Information about the length of the carthesian vector is not
|
||||
/// included in this transformation.
|
||||
static DirectionOf<T> FromVector3(Vector3 vector);
|
||||
|
||||
/// @brief Create a direction using angle values in degrees
|
||||
/// @param horizontal The horizontal angle in degrees
|
||||
/// @param vertical The vertical angle in degrees
|
||||
/// @return The direction
|
||||
static DirectionOf<T> Degrees(float horizontal, float vertical);
|
||||
/// @brief Create a direction using angle values in radians
|
||||
/// @param horizontal The horizontal angle in radians
|
||||
/// @param vertical The vertical angle in radians
|
||||
/// @return The direction
|
||||
static DirectionOf<T> Radians(float horizontal, float vertical);
|
||||
|
||||
/// @brief A forward direction with zero for both angles
|
||||
const static DirectionOf forward;
|
||||
/// @brief A backward direction with horizontal angle -180 and zero vertical
|
||||
/// angle
|
||||
const static DirectionOf back;
|
||||
/// @brief A upward direction with zero horizontal angle and vertical angle 90
|
||||
const static DirectionOf up;
|
||||
/// @brief A downward direction with zero horizontal angle and vertical angle
|
||||
/// -90
|
||||
const static DirectionOf down;
|
||||
/// @brief A left-pointing direction with horizontal angle -90 and zero
|
||||
/// vertical angle
|
||||
const static DirectionOf left;
|
||||
/// @brief A right-pointing direction with horizontal angle 90 and zero
|
||||
/// vertical angle
|
||||
const static DirectionOf right;
|
||||
|
||||
/// @brief Test whether this direction is equal to another direction
|
||||
/// @param direction The direction to compare to
|
||||
/// @return True when the direction angles are equal, false otherwise.
|
||||
bool operator==(const DirectionOf<T> direction) const;
|
||||
|
||||
/// @brief Negate/reverse the direction
|
||||
/// @return The reversed direction.
|
||||
DirectionOf<T> operator-() const;
|
||||
|
||||
protected:
|
||||
/// @brief Normalize this vector to the specified ranges
|
||||
void Normalize();
|
||||
};
|
||||
|
||||
using DirectionSingle = DirectionOf<float>;
|
||||
using Direction16 = DirectionOf<signed short>;
|
||||
|
||||
#if defined(ARDUINO)
|
||||
using Direction = Direction16;
|
||||
#else
|
||||
using Direction = DirectionSingle;
|
||||
#endif
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#endif
|
||||
@ -1,154 +0,0 @@
|
||||
warning: source 'images' is not a readable file or directory... skipping.
|
||||
d:/PlatformIO/linear-algebra/Quaternion.cpp:100: warning: no uniquely matching class member found for
|
||||
Quaternion Quaternion::operator*(const Quaternion &r2) const
|
||||
Possible candidates:
|
||||
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 117 of file d:/PlatformIO/linear-algebra/Angle.h
|
||||
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 124 of file d:/PlatformIO/linear-algebra/Angle.h
|
||||
'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h
|
||||
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 118 of file d:/PlatformIO/linear-algebra/Polar.h
|
||||
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 121 of file d:/PlatformIO/linear-algebra/Polar.h
|
||||
'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 111 of file d:/PlatformIO/linear-algebra/Spherical.h
|
||||
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 114 of file d:/PlatformIO/linear-algebra/Spherical.h
|
||||
'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 46 of file d:/PlatformIO/linear-algebra/SwingTwist.h
|
||||
'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 54 of file d:/PlatformIO/linear-algebra/SwingTwist.h
|
||||
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 141 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 144 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 149 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 152 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
d:/PlatformIO/linear-algebra/Quaternion.cpp:108: warning: no uniquely matching class member found for
|
||||
Vector3 Quaternion::operator*(const Vector3 &p) const
|
||||
Possible candidates:
|
||||
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 117 of file d:/PlatformIO/linear-algebra/Angle.h
|
||||
'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 124 of file d:/PlatformIO/linear-algebra/Angle.h
|
||||
'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h
|
||||
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 118 of file d:/PlatformIO/linear-algebra/Polar.h
|
||||
'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 121 of file d:/PlatformIO/linear-algebra/Polar.h
|
||||
'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 111 of file d:/PlatformIO/linear-algebra/Spherical.h
|
||||
'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 114 of file d:/PlatformIO/linear-algebra/Spherical.h
|
||||
'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 46 of file d:/PlatformIO/linear-algebra/SwingTwist.h
|
||||
'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 54 of file d:/PlatformIO/linear-algebra/SwingTwist.h
|
||||
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 141 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 144 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 149 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 152 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
d:/PlatformIO/linear-algebra/Quaternion.cpp:152: warning: no uniquely matching class member found for
|
||||
Quaternion Quaternion::LookRotation(const Vector3 &forward, const Vector3 &up)
|
||||
Possible candidates:
|
||||
'static Quaternion Passer::LinearAlgebra::Quaternion::LookRotation(const Vector3 &forward, const Vector3 &upwards)' at line 132 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'static Quaternion Passer::LinearAlgebra::Quaternion::LookRotation(const Vector3 &forward)' at line 143 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
d:/PlatformIO/linear-algebra/Quaternion.cpp:330: warning: no uniquely matching class member found for
|
||||
Quaternion Quaternion::Euler(Vector3 euler)
|
||||
Possible candidates:
|
||||
'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(float x, float y, float z)' at line 215 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(Vector3 eulerAngles)' at line 222 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
d:/PlatformIO/linear-algebra/Quaternion.cpp:362: warning: no uniquely matching class member found for
|
||||
Quaternion Quaternion::EulerXYZ(Vector3 euler)
|
||||
Possible candidates:
|
||||
'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(float x, float y, float z)' at line 232 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(Vector3 eulerAngles)' at line 239 of file d:/PlatformIO/linear-algebra/Quaternion.h
|
||||
d:/PlatformIO/linear-algebra/Spherical.cpp:137: warning: no uniquely matching class member found for
|
||||
template < T >
|
||||
SphericalOf< T > SphericalOf::operator-(const SphericalOf< T > &s2) const
|
||||
Possible candidates:
|
||||
'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-() const' at line 99 of file d:/PlatformIO/linear-algebra/Angle.h
|
||||
'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-(const AngleOf< T > &angle) const' at line 103 of file d:/PlatformIO/linear-algebra/Angle.h
|
||||
'DirectionOf< T > Passer::LinearAlgebra::DirectionOf< T >::operator-() const' at line 84 of file d:/PlatformIO/linear-algebra/Direction.h
|
||||
'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-() const' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h
|
||||
'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-(const PolarOf &v) const' at line 105 of file d:/PlatformIO/linear-algebra/Polar.h
|
||||
'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-() const' at line 93 of file d:/PlatformIO/linear-algebra/Spherical.h
|
||||
'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-(const SphericalOf< T > &v) const' at line 98 of file d:/PlatformIO/linear-algebra/Spherical.h
|
||||
'Vector2 Passer::LinearAlgebra::Vector2::operator-()' at line 116 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Vector2 Passer::LinearAlgebra::Vector2::operator-(const Vector2 &v) const' at line 121 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Vector3 Passer::LinearAlgebra::Vector3::operator-() const' at line 124 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
'Vector3 Passer::LinearAlgebra::Vector3::operator-(const Vector3 &v) const' at line 129 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
d:/PlatformIO/linear-algebra/Vector2.cpp:20: warning: no uniquely matching class member found for
|
||||
Vector2::Vector2(float _x, float _y)
|
||||
Possible candidates:
|
||||
'Passer::LinearAlgebra::Vector2::Vector2()' at line 43 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 47 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 51 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 54 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
d:/PlatformIO/linear-algebra/Vector2.cpp:32: warning: no uniquely matching class member found for
|
||||
Vector2::Vector2(PolarSingle p)
|
||||
Possible candidates:
|
||||
'Passer::LinearAlgebra::Vector2::Vector2()' at line 43 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 47 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 51 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 54 of file d:/PlatformIO/linear-algebra/Vector2.h
|
||||
d:/PlatformIO/linear-algebra/Vector3.cpp:33: warning: no uniquely matching class member found for
|
||||
Vector3::Vector3(SphericalOf< float > s)
|
||||
Possible candidates:
|
||||
'Passer::LinearAlgebra::Vector3::Vector3()' at line 47 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
'Passer::LinearAlgebra::Vector3::Vector3(float right, float up, float forward)' at line 52 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
'Passer::LinearAlgebra::Vector3::Vector3(Vector2 v)' at line 55 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
'Passer::LinearAlgebra::Vector3::Vector3(SphericalOf< float > v)' at line 59 of file d:/PlatformIO/linear-algebra/Vector3.h
|
||||
d:/PlatformIO/linear-algebra/Direction.h:43: warning: argument 'v' of command @param is not found in the argument list of Passer::LinearAlgebra::DirectionOf< T >::FromVector3(Vector3 vector)
|
||||
d:/PlatformIO/linear-algebra/Direction.h:43: warning: The following parameter of Passer::LinearAlgebra::DirectionOf::FromVector3(Vector3 vector) is not documented:
|
||||
parameter 'vector'
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:12: warning: Member MatrixOf(unsigned int rows, unsigned int cols) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:13: warning: Member MatrixOf(unsigned int rows, unsigned int cols, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:17: warning: Member MatrixOf(Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:59: warning: Member Multiply(const MatrixOf< T > *m, MatrixOf< T > *r) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:64: warning: Member operator*(const Vector3 v) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:66: warning: Member Get(unsigned int rowIx, unsigned int colIx) const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:71: warning: Member Set(unsigned int rowIx, unsigned int colIx, T value) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:77: warning: Member Set(const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:84: warning: Member SetRow(unsigned int rowIx, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:91: warning: Member SetCol(unsigned int colIx, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:98: warning: Member CopyFrom(const MatrixOf< T > *m) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:108: warning: Member RowCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:109: warning: Member ColCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:57: warning: Member Multiply(const MatrixOf< T > *m1, const MatrixOf< T > *m2, MatrixOf< T > *r) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Matrix.h:63: warning: Member Multiply(const MatrixOf< T > *m, Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:106: warning: Member operator-=(const PolarOf &v) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:111: warning: Member operator+=(const PolarOf &v) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:124: warning: Member operator*=(float f) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:136: warning: Member operator/=(float f) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:121: warning: Member operator*(float f, const PolarOf &v) (friend) of class Passer::LinearAlgebra::PolarOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:133: warning: Member operator/(float f, const PolarOf &v) (friend) of class Passer::LinearAlgebra::PolarOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Polar.h:59: warning: argument 's' of command @param is not found in the argument list of Passer::LinearAlgebra::PolarOf< T >::FromSpherical(SphericalOf< T > v)
|
||||
d:/PlatformIO/linear-algebra/Polar.h:59: warning: The following parameter of Passer::LinearAlgebra::PolarOf::FromSpherical(SphericalOf< T > v) is not documented:
|
||||
parameter 'v'
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:29: warning: Member SphericalOf(float distance, AngleOf< T > horizontal, AngleOf< T > vertical) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:29: warning: Member SphericalOf(float distance, DirectionOf< T > direction) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:99: warning: Member operator-=(const SphericalOf< T > &v) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:104: warning: Member operator+=(const SphericalOf< T > &v) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:117: warning: Member operator*=(float f) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:129: warning: Member operator/=(float f) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:54: warning: Member Rad (variable) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:114: warning: Member operator*(float f, const SphericalOf< T > &v) (friend) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Spherical.h:126: warning: Member operator/(float f, const SphericalOf< T > &v) (friend) of class Passer::LinearAlgebra::SphericalOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member SwingTwistOf(DirectionOf< T > swing, AngleOf< T > twist) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member SwingTwistOf(AngleOf< T > horizontal, AngleOf< T > vertical, AngleOf< T > twist) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:31: warning: Member ToQuaternion() const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:34: warning: Member ToAngleAxis() const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:39: warning: Member operator==(const SwingTwistOf< T > d) const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:55: warning: Member operator*=(const SwingTwistOf< T > &rotation) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:69: warning: Member Normalize() (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:28: warning: Member Degrees(float horizontal, float vertical=0, float twist=0) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:32: warning: Member FromQuaternion(Quaternion q) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:35: warning: Member FromAngleAxis(SphericalOf< T > aa) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:57: warning: Member Inverse(SwingTwistOf< T > rotation) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:67: warning: Member Angle(const SwingTwistOf< T > &r1, const SwingTwistOf< T > &r2) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:21: warning: Member swing (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member twist (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/SwingTwist.h:37: warning: Member identity (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector2.h:122: warning: Member operator-=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector2.h:127: warning: Member operator+=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector2.h:148: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector2.h:159: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector2.h:144: warning: Member operator*(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector2.h:156: warning: Member operator/(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:82: warning: Member Forward() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:83: warning: Member Up() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:84: warning: Member Right() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:130: warning: Member operator-=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:135: warning: Member operator+=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:156: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:168: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:152: warning: Member operator*(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
d:/PlatformIO/linear-algebra/Vector3.h:164: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
|
||||
@ -1,226 +0,0 @@
|
||||
<doxygenlayout version="1.0">
|
||||
<!-- Generated by doxygen 1.8.18 -->
|
||||
<!-- Navigation index tabs for HTML output -->
|
||||
<navindex>
|
||||
<tab type="mainpage" visible="yes" title=""/>
|
||||
<tab type="pages" visible="yes" title="" intro=""/>
|
||||
<tab type="modules" visible="yes" title="" intro=""/>
|
||||
<tab type="namespaces" visible="yes" title="">
|
||||
<tab type="namespacelist" visible="yes" title="" intro=""/>
|
||||
<tab type="namespacemembers" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="interfaces" visible="yes" title="">
|
||||
<tab type="interfacelist" visible="yes" title="" intro=""/>
|
||||
<tab type="interfaceindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
<tab type="interfacehierarchy" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="classes" visible="yes" title="">
|
||||
<tab type="classlist" visible="yes" title="" intro=""/>
|
||||
<tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
<tab type="hierarchy" visible="yes" title="" intro=""/>
|
||||
<tab type="classmembers" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="structs" visible="yes" title="">
|
||||
<tab type="structlist" visible="yes" title="" intro=""/>
|
||||
<tab type="structindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
</tab>
|
||||
<tab type="exceptions" visible="yes" title="">
|
||||
<tab type="exceptionlist" visible="yes" title="" intro=""/>
|
||||
<tab type="exceptionindex" visible="$ALPHABETICAL_INDEX" title=""/>
|
||||
<tab type="exceptionhierarchy" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="files" visible="yes" title="">
|
||||
<tab type="filelist" visible="yes" title="" intro=""/>
|
||||
<tab type="globals" visible="yes" title="" intro=""/>
|
||||
</tab>
|
||||
<tab type="examples" visible="yes" title="" intro=""/>
|
||||
</navindex>
|
||||
|
||||
<!-- Layout definition for a class page -->
|
||||
<class>
|
||||
<briefdescription visible="no"/>
|
||||
<detaileddescription title=""/>
|
||||
<includes visible="$SHOW_INCLUDE_FILES"/>
|
||||
<inheritancegraph visible="$CLASS_GRAPH"/>
|
||||
<collaborationgraph visible="$COLLABORATION_GRAPH"/>
|
||||
<memberdecl>
|
||||
<nestedclasses visible="yes" title=""/>
|
||||
<publictypes title=""/>
|
||||
<services title=""/>
|
||||
<interfaces title=""/>
|
||||
<publicslots title=""/>
|
||||
<signals title=""/>
|
||||
<publicmethods title=""/>
|
||||
<publicstaticmethods title=""/>
|
||||
<publicattributes title=""/>
|
||||
<publicstaticattributes title=""/>
|
||||
<protectedtypes title=""/>
|
||||
<protectedslots title=""/>
|
||||
<protectedmethods title=""/>
|
||||
<protectedstaticmethods title=""/>
|
||||
<protectedattributes title=""/>
|
||||
<protectedstaticattributes title=""/>
|
||||
<packagetypes title=""/>
|
||||
<packagemethods title=""/>
|
||||
<packagestaticmethods title=""/>
|
||||
<packageattributes title=""/>
|
||||
<packagestaticattributes title=""/>
|
||||
<properties title=""/>
|
||||
<events title=""/>
|
||||
<privatetypes title=""/>
|
||||
<privateslots title=""/>
|
||||
<privatemethods title=""/>
|
||||
<privatestaticmethods title=""/>
|
||||
<privateattributes title=""/>
|
||||
<privatestaticattributes title=""/>
|
||||
<friends title=""/>
|
||||
<related title="" subtitle=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<memberdef>
|
||||
<inlineclasses title=""/>
|
||||
<typedefs title=""/>
|
||||
<enums title=""/>
|
||||
<services title=""/>
|
||||
<interfaces title=""/>
|
||||
<constructors title=""/>
|
||||
<functions title=""/>
|
||||
<related title=""/>
|
||||
<variables title=""/>
|
||||
<properties title=""/>
|
||||
<events title=""/>
|
||||
</memberdef>
|
||||
<allmemberslink visible="yes"/>
|
||||
<usedfiles visible="$SHOW_USED_FILES"/>
|
||||
<authorsection visible="yes"/>
|
||||
</class>
|
||||
|
||||
<!-- Layout definition for a namespace page -->
|
||||
<namespace>
|
||||
<briefdescription visible="yes"/>
|
||||
<memberdecl>
|
||||
<nestednamespaces visible="yes" title=""/>
|
||||
<constantgroups visible="yes" title=""/>
|
||||
<interfaces visible="yes" title=""/>
|
||||
<classes visible="yes" title=""/>
|
||||
<structs visible="yes" title=""/>
|
||||
<exceptions visible="yes" title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
<memberdef>
|
||||
<inlineclasses title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
</memberdef>
|
||||
<authorsection visible="yes"/>
|
||||
</namespace>
|
||||
|
||||
<!-- Layout definition for a file page -->
|
||||
<file>
|
||||
<briefdescription visible="yes"/>
|
||||
<includes visible="$SHOW_INCLUDE_FILES"/>
|
||||
<includegraph visible="$INCLUDE_GRAPH"/>
|
||||
<includedbygraph visible="$INCLUDED_BY_GRAPH"/>
|
||||
<sourcelink visible="yes"/>
|
||||
<memberdecl>
|
||||
<interfaces visible="yes" title=""/>
|
||||
<classes visible="yes" title=""/>
|
||||
<structs visible="yes" title=""/>
|
||||
<exceptions visible="yes" title=""/>
|
||||
<namespaces visible="yes" title=""/>
|
||||
<constantgroups visible="yes" title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
<memberdef>
|
||||
<inlineclasses title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
</memberdef>
|
||||
<authorsection/>
|
||||
</file>
|
||||
|
||||
<!-- Layout definition for a group page -->
|
||||
<group>
|
||||
<briefdescription visible="yes"/>
|
||||
<groupgraph visible="$GROUP_GRAPHS"/>
|
||||
<memberdecl>
|
||||
<nestedgroups visible="yes" title=""/>
|
||||
<dirs visible="yes" title=""/>
|
||||
<files visible="yes" title=""/>
|
||||
<namespaces visible="yes" title=""/>
|
||||
<classes visible="yes" title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<enumvalues title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<signals title=""/>
|
||||
<publicslots title=""/>
|
||||
<protectedslots title=""/>
|
||||
<privateslots title=""/>
|
||||
<events title=""/>
|
||||
<properties title=""/>
|
||||
<friends title=""/>
|
||||
<membergroups visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
<memberdef>
|
||||
<pagedocs/>
|
||||
<inlineclasses title=""/>
|
||||
<defines title=""/>
|
||||
<typedefs title=""/>
|
||||
<sequences title=""/>
|
||||
<dictionaries title=""/>
|
||||
<enums title=""/>
|
||||
<enumvalues title=""/>
|
||||
<functions title=""/>
|
||||
<variables title=""/>
|
||||
<signals title=""/>
|
||||
<publicslots title=""/>
|
||||
<protectedslots title=""/>
|
||||
<privateslots title=""/>
|
||||
<events title=""/>
|
||||
<properties title=""/>
|
||||
<friends title=""/>
|
||||
</memberdef>
|
||||
<authorsection visible="yes"/>
|
||||
</group>
|
||||
|
||||
<!-- Layout definition for a directory page -->
|
||||
<directory>
|
||||
<briefdescription visible="yes"/>
|
||||
<directorygraph visible="yes"/>
|
||||
<memberdecl>
|
||||
<dirs visible="yes"/>
|
||||
<files visible="yes"/>
|
||||
</memberdecl>
|
||||
<detaileddescription title=""/>
|
||||
</directory>
|
||||
</doxygenlayout>
|
||||
@ -1,12 +0,0 @@
|
||||
/* Custom PasserVR CSS for DoxyGen */
|
||||
|
||||
a {
|
||||
color: #e77505;
|
||||
}
|
||||
.contents a:visited {
|
||||
color: #e77505;
|
||||
}
|
||||
|
||||
a:hover {
|
||||
color: #10659C;
|
||||
}
|
||||
@ -1,19 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "FloatSingle.h"
|
||||
#include <math.h>
|
||||
|
||||
const float Float::epsilon = 1e-05f;
|
||||
const float Float::sqrEpsilon = 1e-10f;
|
||||
|
||||
float Float::Clamp(float f, float min, float max) {
|
||||
if (max < min)
|
||||
return f;
|
||||
if (f < min)
|
||||
return min;
|
||||
if (f > max)
|
||||
return max;
|
||||
return f;
|
||||
}
|
||||
@ -1,22 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef FLOAT_H
|
||||
#define FLOAT_H
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
class Float {
|
||||
public:
|
||||
static const float epsilon;
|
||||
static const float sqrEpsilon;
|
||||
|
||||
static float Clamp(float f, float min, float max);
|
||||
};
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#endif
|
||||
@ -1,373 +0,0 @@
|
||||
Mozilla Public License Version 2.0
|
||||
==================================
|
||||
|
||||
1. Definitions
|
||||
--------------
|
||||
|
||||
1.1. "Contributor"
|
||||
means each individual or legal entity that creates, contributes to
|
||||
the creation of, or owns Covered Software.
|
||||
|
||||
1.2. "Contributor Version"
|
||||
means the combination of the Contributions of others (if any) used
|
||||
by a Contributor and that particular Contributor's Contribution.
|
||||
|
||||
1.3. "Contribution"
|
||||
means Covered Software of a particular Contributor.
|
||||
|
||||
1.4. "Covered Software"
|
||||
means Source Code Form to which the initial Contributor has attached
|
||||
the notice in Exhibit A, the Executable Form of such Source Code
|
||||
Form, and Modifications of such Source Code Form, in each case
|
||||
including portions thereof.
|
||||
|
||||
1.5. "Incompatible With Secondary Licenses"
|
||||
means
|
||||
|
||||
(a) that the initial Contributor has attached the notice described
|
||||
in Exhibit B to the Covered Software; or
|
||||
|
||||
(b) that the Covered Software was made available under the terms of
|
||||
version 1.1 or earlier of the License, but not also under the
|
||||
terms of a Secondary License.
|
||||
|
||||
1.6. "Executable Form"
|
||||
means any form of the work other than Source Code Form.
|
||||
|
||||
1.7. "Larger Work"
|
||||
means a work that combines Covered Software with other material, in
|
||||
a separate file or files, that is not Covered Software.
|
||||
|
||||
1.8. "License"
|
||||
means this document.
|
||||
|
||||
1.9. "Licensable"
|
||||
means having the right to grant, to the maximum extent possible,
|
||||
whether at the time of the initial grant or subsequently, any and
|
||||
all of the rights conveyed by this License.
|
||||
|
||||
1.10. "Modifications"
|
||||
means any of the following:
|
||||
|
||||
(a) any file in Source Code Form that results from an addition to,
|
||||
deletion from, or modification of the contents of Covered
|
||||
Software; or
|
||||
|
||||
(b) any new file in Source Code Form that contains any Covered
|
||||
Software.
|
||||
|
||||
1.11. "Patent Claims" of a Contributor
|
||||
means any patent claim(s), including without limitation, method,
|
||||
process, and apparatus claims, in any patent Licensable by such
|
||||
Contributor that would be infringed, but for the grant of the
|
||||
License, by the making, using, selling, offering for sale, having
|
||||
made, import, or transfer of either its Contributions or its
|
||||
Contributor Version.
|
||||
|
||||
1.12. "Secondary License"
|
||||
means either the GNU General Public License, Version 2.0, the GNU
|
||||
Lesser General Public License, Version 2.1, the GNU Affero General
|
||||
Public License, Version 3.0, or any later versions of those
|
||||
licenses.
|
||||
|
||||
1.13. "Source Code Form"
|
||||
means the form of the work preferred for making modifications.
|
||||
|
||||
1.14. "You" (or "Your")
|
||||
means an individual or a legal entity exercising rights under this
|
||||
License. For legal entities, "You" includes any entity that
|
||||
controls, is controlled by, or is under common control with You. For
|
||||
purposes of this definition, "control" means (a) the power, direct
|
||||
or indirect, to cause the direction or management of such entity,
|
||||
whether by contract or otherwise, or (b) ownership of more than
|
||||
fifty percent (50%) of the outstanding shares or beneficial
|
||||
ownership of such entity.
|
||||
|
||||
2. License Grants and Conditions
|
||||
--------------------------------
|
||||
|
||||
2.1. Grants
|
||||
|
||||
Each Contributor hereby grants You a world-wide, royalty-free,
|
||||
non-exclusive license:
|
||||
|
||||
(a) under intellectual property rights (other than patent or trademark)
|
||||
Licensable by such Contributor to use, reproduce, make available,
|
||||
modify, display, perform, distribute, and otherwise exploit its
|
||||
Contributions, either on an unmodified basis, with Modifications, or
|
||||
as part of a Larger Work; and
|
||||
|
||||
(b) under Patent Claims of such Contributor to make, use, sell, offer
|
||||
for sale, have made, import, and otherwise transfer either its
|
||||
Contributions or its Contributor Version.
|
||||
|
||||
2.2. Effective Date
|
||||
|
||||
The licenses granted in Section 2.1 with respect to any Contribution
|
||||
become effective for each Contribution on the date the Contributor first
|
||||
distributes such Contribution.
|
||||
|
||||
2.3. Limitations on Grant Scope
|
||||
|
||||
The licenses granted in this Section 2 are the only rights granted under
|
||||
this License. No additional rights or licenses will be implied from the
|
||||
distribution or licensing of Covered Software under this License.
|
||||
Notwithstanding Section 2.1(b) above, no patent license is granted by a
|
||||
Contributor:
|
||||
|
||||
(a) for any code that a Contributor has removed from Covered Software;
|
||||
or
|
||||
|
||||
(b) for infringements caused by: (i) Your and any other third party's
|
||||
modifications of Covered Software, or (ii) the combination of its
|
||||
Contributions with other software (except as part of its Contributor
|
||||
Version); or
|
||||
|
||||
(c) under Patent Claims infringed by Covered Software in the absence of
|
||||
its Contributions.
|
||||
|
||||
This License does not grant any rights in the trademarks, service marks,
|
||||
or logos of any Contributor (except as may be necessary to comply with
|
||||
the notice requirements in Section 3.4).
|
||||
|
||||
2.4. Subsequent Licenses
|
||||
|
||||
No Contributor makes additional grants as a result of Your choice to
|
||||
distribute the Covered Software under a subsequent version of this
|
||||
License (see Section 10.2) or under the terms of a Secondary License (if
|
||||
permitted under the terms of Section 3.3).
|
||||
|
||||
2.5. Representation
|
||||
|
||||
Each Contributor represents that the Contributor believes its
|
||||
Contributions are its original creation(s) or it has sufficient rights
|
||||
to grant the rights to its Contributions conveyed by this License.
|
||||
|
||||
2.6. Fair Use
|
||||
|
||||
This License is not intended to limit any rights You have under
|
||||
applicable copyright doctrines of fair use, fair dealing, or other
|
||||
equivalents.
|
||||
|
||||
2.7. Conditions
|
||||
|
||||
Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
|
||||
in Section 2.1.
|
||||
|
||||
3. Responsibilities
|
||||
-------------------
|
||||
|
||||
3.1. Distribution of Source Form
|
||||
|
||||
All distribution of Covered Software in Source Code Form, including any
|
||||
Modifications that You create or to which You contribute, must be under
|
||||
the terms of this License. You must inform recipients that the Source
|
||||
Code Form of the Covered Software is governed by the terms of this
|
||||
License, and how they can obtain a copy of this License. You may not
|
||||
attempt to alter or restrict the recipients' rights in the Source Code
|
||||
Form.
|
||||
|
||||
3.2. Distribution of Executable Form
|
||||
|
||||
If You distribute Covered Software in Executable Form then:
|
||||
|
||||
(a) such Covered Software must also be made available in Source Code
|
||||
Form, as described in Section 3.1, and You must inform recipients of
|
||||
the Executable Form how they can obtain a copy of such Source Code
|
||||
Form by reasonable means in a timely manner, at a charge no more
|
||||
than the cost of distribution to the recipient; and
|
||||
|
||||
(b) You may distribute such Executable Form under the terms of this
|
||||
License, or sublicense it under different terms, provided that the
|
||||
license for the Executable Form does not attempt to limit or alter
|
||||
the recipients' rights in the Source Code Form under this License.
|
||||
|
||||
3.3. Distribution of a Larger Work
|
||||
|
||||
You may create and distribute a Larger Work under terms of Your choice,
|
||||
provided that You also comply with the requirements of this License for
|
||||
the Covered Software. If the Larger Work is a combination of Covered
|
||||
Software with a work governed by one or more Secondary Licenses, and the
|
||||
Covered Software is not Incompatible With Secondary Licenses, this
|
||||
License permits You to additionally distribute such Covered Software
|
||||
under the terms of such Secondary License(s), so that the recipient of
|
||||
the Larger Work may, at their option, further distribute the Covered
|
||||
Software under the terms of either this License or such Secondary
|
||||
License(s).
|
||||
|
||||
3.4. Notices
|
||||
|
||||
You may not remove or alter the substance of any license notices
|
||||
(including copyright notices, patent notices, disclaimers of warranty,
|
||||
or limitations of liability) contained within the Source Code Form of
|
||||
the Covered Software, except that You may alter any license notices to
|
||||
the extent required to remedy known factual inaccuracies.
|
||||
|
||||
3.5. Application of Additional Terms
|
||||
|
||||
You may choose to offer, and to charge a fee for, warranty, support,
|
||||
indemnity or liability obligations to one or more recipients of Covered
|
||||
Software. However, You may do so only on Your own behalf, and not on
|
||||
behalf of any Contributor. You must make it absolutely clear that any
|
||||
such warranty, support, indemnity, or liability obligation is offered by
|
||||
You alone, and You hereby agree to indemnify every Contributor for any
|
||||
liability incurred by such Contributor as a result of warranty, support,
|
||||
indemnity or liability terms You offer. You may include additional
|
||||
disclaimers of warranty and limitations of liability specific to any
|
||||
jurisdiction.
|
||||
|
||||
4. Inability to Comply Due to Statute or Regulation
|
||||
---------------------------------------------------
|
||||
|
||||
If it is impossible for You to comply with any of the terms of this
|
||||
License with respect to some or all of the Covered Software due to
|
||||
statute, judicial order, or regulation then You must: (a) comply with
|
||||
the terms of this License to the maximum extent possible; and (b)
|
||||
describe the limitations and the code they affect. Such description must
|
||||
be placed in a text file included with all distributions of the Covered
|
||||
Software under this License. Except to the extent prohibited by statute
|
||||
or regulation, such description must be sufficiently detailed for a
|
||||
recipient of ordinary skill to be able to understand it.
|
||||
|
||||
5. Termination
|
||||
--------------
|
||||
|
||||
5.1. The rights granted under this License will terminate automatically
|
||||
if You fail to comply with any of its terms. However, if You become
|
||||
compliant, then the rights granted under this License from a particular
|
||||
Contributor are reinstated (a) provisionally, unless and until such
|
||||
Contributor explicitly and finally terminates Your grants, and (b) on an
|
||||
ongoing basis, if such Contributor fails to notify You of the
|
||||
non-compliance by some reasonable means prior to 60 days after You have
|
||||
come back into compliance. Moreover, Your grants from a particular
|
||||
Contributor are reinstated on an ongoing basis if such Contributor
|
||||
notifies You of the non-compliance by some reasonable means, this is the
|
||||
first time You have received notice of non-compliance with this License
|
||||
from such Contributor, and You become compliant prior to 30 days after
|
||||
Your receipt of the notice.
|
||||
|
||||
5.2. If You initiate litigation against any entity by asserting a patent
|
||||
infringement claim (excluding declaratory judgment actions,
|
||||
counter-claims, and cross-claims) alleging that a Contributor Version
|
||||
directly or indirectly infringes any patent, then the rights granted to
|
||||
You by any and all Contributors for the Covered Software under Section
|
||||
2.1 of this License shall terminate.
|
||||
|
||||
5.3. In the event of termination under Sections 5.1 or 5.2 above, all
|
||||
end user license agreements (excluding distributors and resellers) which
|
||||
have been validly granted by You or Your distributors under this License
|
||||
prior to termination shall survive termination.
|
||||
|
||||
************************************************************************
|
||||
* *
|
||||
* 6. Disclaimer of Warranty *
|
||||
* ------------------------- *
|
||||
* *
|
||||
* Covered Software is provided under this License on an "as is" *
|
||||
* basis, without warranty of any kind, either expressed, implied, or *
|
||||
* statutory, including, without limitation, warranties that the *
|
||||
* Covered Software is free of defects, merchantable, fit for a *
|
||||
* particular purpose or non-infringing. The entire risk as to the *
|
||||
* quality and performance of the Covered Software is with You. *
|
||||
* Should any Covered Software prove defective in any respect, You *
|
||||
* (not any Contributor) assume the cost of any necessary servicing, *
|
||||
* repair, or correction. This disclaimer of warranty constitutes an *
|
||||
* essential part of this License. No use of any Covered Software is *
|
||||
* authorized under this License except under this disclaimer. *
|
||||
* *
|
||||
************************************************************************
|
||||
|
||||
************************************************************************
|
||||
* *
|
||||
* 7. Limitation of Liability *
|
||||
* -------------------------- *
|
||||
* *
|
||||
* Under no circumstances and under no legal theory, whether tort *
|
||||
* (including negligence), contract, or otherwise, shall any *
|
||||
* Contributor, or anyone who distributes Covered Software as *
|
||||
* permitted above, be liable to You for any direct, indirect, *
|
||||
* special, incidental, or consequential damages of any character *
|
||||
* including, without limitation, damages for lost profits, loss of *
|
||||
* goodwill, work stoppage, computer failure or malfunction, or any *
|
||||
* and all other commercial damages or losses, even if such party *
|
||||
* shall have been informed of the possibility of such damages. This *
|
||||
* limitation of liability shall not apply to liability for death or *
|
||||
* personal injury resulting from such party's negligence to the *
|
||||
* extent applicable law prohibits such limitation. Some *
|
||||
* jurisdictions do not allow the exclusion or limitation of *
|
||||
* incidental or consequential damages, so this exclusion and *
|
||||
* limitation may not apply to You. *
|
||||
* *
|
||||
************************************************************************
|
||||
|
||||
8. Litigation
|
||||
-------------
|
||||
|
||||
Any litigation relating to this License may be brought only in the
|
||||
courts of a jurisdiction where the defendant maintains its principal
|
||||
place of business and such litigation shall be governed by laws of that
|
||||
jurisdiction, without reference to its conflict-of-law provisions.
|
||||
Nothing in this Section shall prevent a party's ability to bring
|
||||
cross-claims or counter-claims.
|
||||
|
||||
9. Miscellaneous
|
||||
----------------
|
||||
|
||||
This License represents the complete agreement concerning the subject
|
||||
matter hereof. If any provision of this License is held to be
|
||||
unenforceable, such provision shall be reformed only to the extent
|
||||
necessary to make it enforceable. Any law or regulation which provides
|
||||
that the language of a contract shall be construed against the drafter
|
||||
shall not be used to construe this License against a Contributor.
|
||||
|
||||
10. Versions of the License
|
||||
---------------------------
|
||||
|
||||
10.1. New Versions
|
||||
|
||||
Mozilla Foundation is the license steward. Except as provided in Section
|
||||
10.3, no one other than the license steward has the right to modify or
|
||||
publish new versions of this License. Each version will be given a
|
||||
distinguishing version number.
|
||||
|
||||
10.2. Effect of New Versions
|
||||
|
||||
You may distribute the Covered Software under the terms of the version
|
||||
of the License under which You originally received the Covered Software,
|
||||
or under the terms of any subsequent version published by the license
|
||||
steward.
|
||||
|
||||
10.3. Modified Versions
|
||||
|
||||
If you create software not governed by this License, and you want to
|
||||
create a new license for such software, you may create and use a
|
||||
modified version of this License if you rename the license and remove
|
||||
any references to the name of the license steward (except to note that
|
||||
such modified license differs from this License).
|
||||
|
||||
10.4. Distributing Source Code Form that is Incompatible With Secondary
|
||||
Licenses
|
||||
|
||||
If You choose to distribute Source Code Form that is Incompatible With
|
||||
Secondary Licenses under the terms of this version of the License, the
|
||||
notice described in Exhibit B of this License must be attached.
|
||||
|
||||
Exhibit A - Source Code Form License Notice
|
||||
-------------------------------------------
|
||||
|
||||
This Source Code Form is subject to the terms of the Mozilla Public
|
||||
License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
If it is not possible or desirable to put the notice in a particular
|
||||
file, then You may include the notice in a location (such as a LICENSE
|
||||
file in a relevant directory) where a recipient would be likely to look
|
||||
for such a notice.
|
||||
|
||||
You may add additional accurate notices of copyright ownership.
|
||||
|
||||
Exhibit B - "Incompatible With Secondary Licenses" Notice
|
||||
---------------------------------------------------------
|
||||
|
||||
This Source Code Form is "Incompatible With Secondary Licenses", as
|
||||
defined by the Mozilla Public License, v. 2.0.
|
||||
@ -1,62 +0,0 @@
|
||||
#include "Matrix.h"
|
||||
|
||||
template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
|
||||
if (rows <= 0 || cols <= 0) {
|
||||
this->rows = 0;
|
||||
this->cols = 0;
|
||||
this->data = nullptr;
|
||||
return;
|
||||
}
|
||||
this->rows = rows;
|
||||
this->cols = cols;
|
||||
|
||||
unsigned int matrixSize = this->cols * this->rows;
|
||||
this->data = new float[matrixSize]{0.0f};
|
||||
}
|
||||
|
||||
template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
|
||||
Set(0, 0, v.Right());
|
||||
Set(1, 0, v.Up());
|
||||
Set(2, 0, v.Forward());
|
||||
}
|
||||
|
||||
template <>
|
||||
void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
|
||||
const MatrixOf<float> *m2, MatrixOf<float> *r) {
|
||||
for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
|
||||
for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
|
||||
unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
|
||||
r->data[rDataIx] = 0.0F;
|
||||
for (unsigned int kIx = 0; kIx < m2->rows; kIx++) {
|
||||
unsigned int dataIx1 = rowIx1 * m1->cols + kIx;
|
||||
unsigned int dataIx2 = kIx * m2->cols + colIx2;
|
||||
r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <>
|
||||
Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
|
||||
MatrixOf<float> v_m = MatrixOf<float>(v);
|
||||
MatrixOf<float> r_m = MatrixOf<float>(3, 1);
|
||||
|
||||
Multiply(m, &v_m, &r_m);
|
||||
|
||||
Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
|
||||
float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
|
||||
MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
|
||||
float *rData = new float[3]{};
|
||||
MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
|
||||
|
||||
Multiply(this, &v_m, &r_m);
|
||||
|
||||
Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
|
||||
delete[] vData;
|
||||
delete[] rData;
|
||||
return r;
|
||||
}
|
||||
@ -1,121 +0,0 @@
|
||||
#ifndef MATRIX_H
|
||||
#define MATRIX_H
|
||||
|
||||
#include "Vector3.h"
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
/// @brief Single precision float matrix
|
||||
template <typename T>
|
||||
class MatrixOf {
|
||||
public:
|
||||
MatrixOf(unsigned int rows, unsigned int cols);
|
||||
MatrixOf(unsigned int rows, unsigned int cols, const T* source)
|
||||
: MatrixOf(rows, cols) {
|
||||
Set(source);
|
||||
}
|
||||
MatrixOf(Vector3 v); // creates a 3,1 matrix
|
||||
|
||||
~MatrixOf() {
|
||||
if (this->data == nullptr)
|
||||
return;
|
||||
|
||||
delete[] this->data;
|
||||
}
|
||||
|
||||
/// @brief Transpose with result in matrix m
|
||||
/// @param r The matrix in which the transposed matrix is stored
|
||||
void Transpose(MatrixOf<T>* r) const {
|
||||
// Check dimensions first
|
||||
// We dont care about the rows and cols (we overwrite them)
|
||||
// but the data size should be equal to avoid problems
|
||||
// We cannot check the data size directly, but the row*col should be equal
|
||||
unsigned int matrixSize = this->cols * this->rows;
|
||||
unsigned int resultSize = r->rows * r->cols;
|
||||
if (matrixSize != resultSize) {
|
||||
// Return a null matrix;
|
||||
// We dont set data to nullptr because it is allocated memory
|
||||
// Instead we write all zeros
|
||||
for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
|
||||
r->data[dataIx] = 0.0f;
|
||||
r->rows = 0;
|
||||
r->cols = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
r->cols = this->rows;
|
||||
r->rows = this->cols;
|
||||
|
||||
for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
|
||||
unsigned int rowIx = rDataIx / this->rows;
|
||||
unsigned int colIx = rDataIx % this->rows;
|
||||
unsigned int mDataIx = this->cols * colIx + rowIx;
|
||||
r->data[rDataIx] = this->data[mDataIx];
|
||||
}
|
||||
}
|
||||
|
||||
static void Multiply(const MatrixOf<T>* m1,
|
||||
const MatrixOf<T>* m2,
|
||||
MatrixOf<T>* r);
|
||||
void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
|
||||
Multiply(this, m, r);
|
||||
}
|
||||
|
||||
static Vector3 Multiply(const MatrixOf<T>* m, Vector3 v);
|
||||
Vector3 operator*(const Vector3 v) const;
|
||||
|
||||
T Get(unsigned int rowIx, unsigned int colIx) const {
|
||||
unsigned int dataIx = rowIx * this->cols + colIx;
|
||||
return this->data[dataIx];
|
||||
}
|
||||
|
||||
void Set(unsigned int rowIx, unsigned int colIx, T value) {
|
||||
unsigned int dataIx = rowIx * this->cols + colIx;
|
||||
this->data[dataIx] = value;
|
||||
}
|
||||
|
||||
// This function does not check on source size!
|
||||
void Set(const T* source) {
|
||||
unsigned int matrixSize = this->cols * this->rows;
|
||||
for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
|
||||
this->data[dataIx] = source[dataIx];
|
||||
}
|
||||
|
||||
// This function does not check on source size!
|
||||
void SetRow(unsigned int rowIx, const T* source) {
|
||||
unsigned int dataIx = rowIx * this->cols;
|
||||
for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
|
||||
this->data[dataIx] = source[sourceIx];
|
||||
}
|
||||
|
||||
// This function does not check on source size!
|
||||
void SetCol(unsigned int colIx, const T* source) {
|
||||
unsigned int dataIx = colIx;
|
||||
for (unsigned int sourceIx = 0; sourceIx < this->cols;
|
||||
dataIx += this->cols, sourceIx++)
|
||||
this->data[dataIx] = source[sourceIx];
|
||||
}
|
||||
|
||||
void CopyFrom(const MatrixOf<T>* m) {
|
||||
unsigned int thisMatrixSize = this->cols * this->rows;
|
||||
unsigned int mMatrixSize = m->cols * m->rows;
|
||||
if (mMatrixSize != thisMatrixSize)
|
||||
return;
|
||||
|
||||
for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
|
||||
this->data[dataIx] = m->data[dataIx];
|
||||
}
|
||||
|
||||
unsigned int RowCount() const { return rows; }
|
||||
unsigned int ColCount() const { return cols; }
|
||||
|
||||
private:
|
||||
unsigned int rows;
|
||||
unsigned int cols;
|
||||
T* data;
|
||||
};
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#endif
|
||||
@ -1,179 +0,0 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "Polar.h"
|
||||
#include "Vector2.h"
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T>::PolarOf() {
|
||||
this->distance = 0.0f;
|
||||
this->angle = AngleOf<T>();
|
||||
}
|
||||
template <typename T>
|
||||
PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
|
||||
// distance should always be 0 or greater
|
||||
if (distance < 0.0f) {
|
||||
this->distance = -distance;
|
||||
this->angle = AngleOf<T>::Normalize(angle - AngleOf<T>::Degrees(180));
|
||||
} else {
|
||||
this->distance = distance;
|
||||
if (this->distance == 0.0f)
|
||||
// angle is always 0 if distance is 0
|
||||
this->angle = AngleOf<T>();
|
||||
else
|
||||
this->angle = AngleOf<T>::Normalize(angle);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::Degrees(float distance, float degrees) {
|
||||
AngleOf<T> angle = AngleOf<T>::Degrees(degrees);
|
||||
PolarOf<T> r = PolarOf<T>(distance, angle);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::Radians(float distance, float radians) {
|
||||
return PolarOf<T>(distance, AngleOf<T>::Radians(radians));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
|
||||
float distance = v.magnitude();
|
||||
AngleOf<T> angle =
|
||||
AngleOf<T>::Degrees(Vector2::SignedAngle(Vector2::forward, v));
|
||||
PolarOf<T> p = PolarOf(distance, angle);
|
||||
return p;
|
||||
}
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
|
||||
float distance =
|
||||
v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
|
||||
AngleOf<T> angle = v.direction.horizontal;
|
||||
PolarOf<T> p = PolarOf(distance, angle);
|
||||
return p;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
const PolarOf<T> PolarOf<T>::zero = PolarOf(0.0f, AngleOf<T>());
|
||||
template <typename T>
|
||||
const PolarOf<T> PolarOf<T>::forward = PolarOf(1.0f, AngleOf<T>());
|
||||
template <typename T>
|
||||
const PolarOf<T> PolarOf<T>::back = PolarOf(1.0, AngleOf<T>::Degrees(180));
|
||||
template <typename T>
|
||||
const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, AngleOf<T>::Degrees(90));
|
||||
template <typename T>
|
||||
const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, AngleOf<T>::Degrees(-90));
|
||||
|
||||
template <typename T>
|
||||
bool PolarOf<T>::operator==(const PolarOf& v) const {
|
||||
return (this->distance == v.distance &&
|
||||
this->angle.InDegrees() == v.angle.InDegrees());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
|
||||
PolarOf<T> r = PolarOf(1, v.angle);
|
||||
return r;
|
||||
}
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::normalized() const {
|
||||
PolarOf<T> r = PolarOf(1, this->angle);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator-() const {
|
||||
PolarOf<T> v =
|
||||
PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
|
||||
PolarOf<T> r = -v;
|
||||
return *this + r;
|
||||
}
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
|
||||
*this = *this - v;
|
||||
return *this;
|
||||
// angle = AngleOf<T>::Normalize(newAngle);
|
||||
// distance = newDistance;
|
||||
}
|
||||
|
||||
// Polar::Polar(Vector2 v) {
|
||||
// float signY = (v.y >= 0) - (v.y < 0);
|
||||
// angle = atan2(v.y, signY * sqrt(v.y * v.y + v.x * v.x)) * Angle::Rad2Deg;
|
||||
// distance = v.magnitude();
|
||||
// }
|
||||
|
||||
// const Polar Polar::zero = Polar(0, 0);
|
||||
|
||||
// float Polar::Distance(const Polar &v1, const Polar &v2) {
|
||||
// float d =
|
||||
// Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
|
||||
// return d;
|
||||
// }
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
|
||||
if (v.distance == 0)
|
||||
return PolarOf(this->distance, this->angle);
|
||||
if (this->distance == 0.0f)
|
||||
return v;
|
||||
|
||||
float deltaAngle = AngleOf<T>::Normalize(v.angle - this->angle).InDegrees();
|
||||
float rotation =
|
||||
deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
|
||||
|
||||
if (rotation == 180.0f && v.distance > 0.0f) {
|
||||
// angle is too small, take this angle and add the distances
|
||||
return PolarOf(this->distance + v.distance, this->angle);
|
||||
}
|
||||
|
||||
float newDistance = AngleOf<T>::CosineRuleSide(v.distance, this->distance,
|
||||
AngleOf<T>::Degrees(rotation));
|
||||
|
||||
float angle =
|
||||
AngleSingle::CosineRuleAngle(newDistance, this->distance, v.distance)
|
||||
.InDegrees();
|
||||
|
||||
float newAngle = deltaAngle < 0.0f ? this->angle.InDegrees() - angle
|
||||
: this->angle.InDegrees() + angle;
|
||||
AngleOf<T> newAngleA = AngleOf<T>::Normalize(AngleOf<T>::Degrees(newAngle));
|
||||
PolarOf vector = PolarOf(newDistance, newAngleA);
|
||||
return vector;
|
||||
}
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
|
||||
*this = *this + v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator*=(float f) {
|
||||
this->distance *= f;
|
||||
return *this;
|
||||
}
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::operator/=(float f) {
|
||||
this->distance /= f;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
float PolarOf<T>::Distance(const PolarOf& v1, const PolarOf& v2) {
|
||||
float d =
|
||||
AngleOf<T>::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
|
||||
return d;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
|
||||
AngleOf<T> a = AngleOf<T>::Normalize(v.angle + angle);
|
||||
PolarOf<T> r = PolarOf(v.distance, a);
|
||||
return r;
|
||||
}
|
||||
|
||||
template class PolarOf<float>;
|
||||
template class PolarOf<signed short>;
|
||||
@ -1,162 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef POLAR_H
|
||||
#define POLAR_H
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
struct Vector2;
|
||||
template <typename T>
|
||||
class SphericalOf;
|
||||
|
||||
/// @brief A polar vector using an angle in various representations
|
||||
/// @tparam T The implementation type used for the representation of the angle
|
||||
template <typename T>
|
||||
class PolarOf {
|
||||
public:
|
||||
/// @brief The distance in meters
|
||||
/// @remark The distance shall never be negative
|
||||
float distance;
|
||||
/// @brief The angle in degrees clockwise rotation
|
||||
/// @remark The angle shall be between -180 .. 180
|
||||
AngleOf<T> angle;
|
||||
|
||||
/// @brief A new vector with polar coordinates with zero degrees and
|
||||
/// distance
|
||||
PolarOf();
|
||||
/// @brief A new vector with polar coordinates
|
||||
/// @param distance The distance in meters
|
||||
/// @param angle The angle in degrees, clockwise rotation
|
||||
/// @note The distance is automatically converted to a positive value.
|
||||
/// @note The angle is automatically normalized to -180 .. 180
|
||||
PolarOf(float distance, AngleOf<T> angle);
|
||||
|
||||
/// @brief Create polar vector without using AngleOf type. All given angles
|
||||
/// are in degrees
|
||||
/// @param distance The distance in meters
|
||||
/// @param degrees The angle in degrees
|
||||
/// @return The polar vector
|
||||
static PolarOf<T> Degrees(float distance, float degrees);
|
||||
/// @brief Short-hand Deg alias for the Degrees function
|
||||
constexpr static auto Deg = Degrees;
|
||||
/// @brief Create polar vector without using AngleOf type. All given angles
|
||||
/// are in radians.
|
||||
/// @param distance The distance in meters
|
||||
/// @param radians The angle in radians
|
||||
/// @return The polar vector
|
||||
static PolarOf<T> Radians(float distance, float radians);
|
||||
/// @brief Short-hand Rad alias for the Radians function
|
||||
constexpr static auto Rad = Radians;
|
||||
|
||||
/// @brief Convert a vector from 2D carthesian coordinates to polar
|
||||
/// coordinates
|
||||
/// @param v The vector to convert
|
||||
static PolarOf<T> FromVector2(Vector2 v);
|
||||
/// @brief Convert a vector from spherical coordinates to polar coordinates
|
||||
/// @param s The vector to convert
|
||||
/// @note The resulting vector will be projected on the horizontal plane
|
||||
static PolarOf<T> FromSpherical(SphericalOf<T> v);
|
||||
|
||||
/// @brief A polar vector with zero degrees and distance
|
||||
const static PolarOf zero;
|
||||
/// @brief A normalized forward-oriented vector
|
||||
const static PolarOf forward;
|
||||
/// @brief A normalized back-oriented vector
|
||||
const static PolarOf back;
|
||||
/// @brief A normalized right-oriented vector
|
||||
const static PolarOf right;
|
||||
/// @brief A normalized left-oriented vector
|
||||
const static PolarOf left;
|
||||
|
||||
/// @brief Equality test to another vector
|
||||
/// @param v The vector to check against
|
||||
/// @return true: if it is identical to the given vector
|
||||
/// @note This uses float comparison to check equality which may have
|
||||
/// strange effects. Equality on floats should be avoided.
|
||||
bool operator==(const PolarOf& v) const;
|
||||
|
||||
/// @brief The vector length
|
||||
/// @param v The vector for which you need the length
|
||||
/// @return The vector length;
|
||||
inline static float Magnitude(const PolarOf& v) { return v.distance; }
|
||||
/// @brief The vector length
|
||||
/// @return The vector length
|
||||
inline float magnitude() const { return this->distance; }
|
||||
|
||||
/// @brief Convert the vector to a length of 1
|
||||
/// @param v The vector to convert
|
||||
/// @return The vector normalized to a length of 1
|
||||
static PolarOf Normalize(const PolarOf& v);
|
||||
/// @brief Convert the vector to a length of a
|
||||
/// @return The vector normalized to a length of 1
|
||||
PolarOf normalized() const;
|
||||
|
||||
/// @brief Negate the vector
|
||||
/// @return The negated vector
|
||||
/// This will rotate the vector by 180 degrees. Distance will stay the same.
|
||||
PolarOf operator-() const;
|
||||
|
||||
/// @brief Subtract a polar vector from this vector
|
||||
/// @param v The vector to subtract
|
||||
/// @return The result of the subtraction
|
||||
PolarOf operator-(const PolarOf& v) const;
|
||||
PolarOf operator-=(const PolarOf& v);
|
||||
/// @brief Add a polar vector to this vector
|
||||
/// @param v The vector to add
|
||||
/// @return The result of the addition
|
||||
PolarOf operator+(const PolarOf& v) const;
|
||||
PolarOf operator+=(const PolarOf& v);
|
||||
|
||||
/// @brief Scale the vector uniformly up
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled vector
|
||||
/// @remark This operation will scale the distance of the vector. The angle
|
||||
/// will be unaffected.
|
||||
friend PolarOf operator*(const PolarOf& v, float f) {
|
||||
return PolarOf(v.distance * f, v.angle);
|
||||
}
|
||||
friend PolarOf operator*(float f, const PolarOf& v) {
|
||||
return PolarOf(f * v.distance, v.angle);
|
||||
}
|
||||
PolarOf operator*=(float f);
|
||||
/// @brief Scale the vector uniformly down
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled factor
|
||||
/// @remark This operation will scale the distance of the vector. The angle
|
||||
/// will be unaffected.
|
||||
friend PolarOf operator/(const PolarOf& v, float f) {
|
||||
return PolarOf(v.distance / f, v.angle);
|
||||
}
|
||||
friend PolarOf operator/(float f, const PolarOf& v) {
|
||||
return PolarOf(f / v.distance, v.angle);
|
||||
}
|
||||
PolarOf operator/=(float f);
|
||||
|
||||
/// @brief The distance between two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The distance between the two vectors
|
||||
static float Distance(const PolarOf& v1, const PolarOf& v2);
|
||||
|
||||
/// @brief Rotate a vector
|
||||
/// @param v The vector to rotate
|
||||
/// @param a The angle in degreesto rotate
|
||||
/// @return The rotated vector
|
||||
static PolarOf Rotate(const PolarOf& v, AngleOf<T> a);
|
||||
};
|
||||
|
||||
using PolarSingle = PolarOf<float>;
|
||||
using Polar16 = PolarOf<signed short>;
|
||||
// using Polar = PolarSingle;
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#include "Spherical.h"
|
||||
#include "Vector2.h"
|
||||
|
||||
#endif
|
||||
@ -1,418 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "Quaternion.h"
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include "Angle.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
void CopyQuat(const Quat& q1, Quat& q2) {
|
||||
q2.x = q1.x;
|
||||
q2.y = q1.y;
|
||||
q2.z = q1.z;
|
||||
q2.w = q1.w;
|
||||
}
|
||||
|
||||
const float Deg2Rad = 0.0174532924F;
|
||||
const float Rad2Deg = 57.29578F;
|
||||
|
||||
Quaternion::Quaternion() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
z = 0;
|
||||
w = 1;
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(float _x, float _y, float _z, float _w) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
w = _w;
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(Quat q) {
|
||||
x = q.x;
|
||||
y = q.y;
|
||||
z = q.z;
|
||||
w = q.w;
|
||||
}
|
||||
|
||||
Quaternion::~Quaternion() {}
|
||||
|
||||
const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1);
|
||||
|
||||
Vector3 Quaternion::xyz() const {
|
||||
return Vector3(x, y, z);
|
||||
}
|
||||
|
||||
float Quaternion::GetLength() const {
|
||||
return sqrtf(x * x + y * y + z * z + w * w);
|
||||
}
|
||||
|
||||
float Quaternion::GetLengthSquared() const {
|
||||
return x * x + y * y + z * z + w * w;
|
||||
}
|
||||
float Quaternion::GetLengthSquared(const Quaternion& q) {
|
||||
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
|
||||
}
|
||||
|
||||
void Quaternion::Normalize() {
|
||||
float length = GetLength();
|
||||
x /= length;
|
||||
y /= length;
|
||||
z /= length;
|
||||
w /= length;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Normalize(const Quaternion& q) {
|
||||
Quaternion result;
|
||||
float length = q.GetLength();
|
||||
result = Quaternion(q.x / length, q.y / length, q.z / length, q.w / length);
|
||||
return result;
|
||||
};
|
||||
|
||||
float Quaternion::Dot(Quaternion a, Quaternion b) {
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
}
|
||||
|
||||
Vector3 Quaternion::ToAngles(const Quaternion& q1) {
|
||||
float test = q1.x * q1.y + q1.z * q1.w;
|
||||
if (test > 0.499f) { // singularity at north pole
|
||||
return Vector3(0, 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, 90);
|
||||
} else if (test < -0.499f) { // singularity at south pole
|
||||
return Vector3(0, -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, -90);
|
||||
} else {
|
||||
float sqx = q1.x * q1.x;
|
||||
float sqy = q1.y * q1.y;
|
||||
float sqz = q1.z * q1.z;
|
||||
|
||||
return Vector3(
|
||||
atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) *
|
||||
Rad2Deg,
|
||||
atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) *
|
||||
Rad2Deg,
|
||||
asinf(2 * test) * Rad2Deg);
|
||||
}
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const Quaternion& r2) const {
|
||||
return Quaternion(
|
||||
this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,
|
||||
-this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y,
|
||||
this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z,
|
||||
-this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w);
|
||||
};
|
||||
|
||||
Vector3 Quaternion::operator*(const Vector3& p) const {
|
||||
float num = this->x * 2;
|
||||
float num2 = this->y * 2;
|
||||
float num3 = this->z * 2;
|
||||
float num4 = this->x * num;
|
||||
float num5 = this->y * num2;
|
||||
float num6 = this->z * num3;
|
||||
float num7 = this->x * num2;
|
||||
float num8 = this->x * num3;
|
||||
float num9 = this->y * num3;
|
||||
float num10 = this->w * num;
|
||||
float num11 = this->w * num2;
|
||||
float num12 = this->w * num3;
|
||||
|
||||
float px = p.Right();
|
||||
float py = p.Up();
|
||||
float pz = p.Forward();
|
||||
// Vector3 result = Vector3::zero;
|
||||
// result.x =
|
||||
float rx =
|
||||
(1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
|
||||
// result.y =
|
||||
float ry =
|
||||
(num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
|
||||
// result.z =
|
||||
float rz =
|
||||
(num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
|
||||
Vector3 result = Vector3(rx, ry, rz);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Quaternion::operator==(const Quaternion& q) const {
|
||||
return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Inverse(Quaternion r) {
|
||||
float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w);
|
||||
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::LookRotation(const Vector3& forward) {
|
||||
Vector3 up = Vector3(0, 1, 0);
|
||||
return LookRotation(forward, up);
|
||||
}
|
||||
Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) {
|
||||
Vector3 nForward = Vector3::Normalize(forward);
|
||||
Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward));
|
||||
Vector3 nUp = Vector3::Cross(nForward, nRight);
|
||||
float m00 = nRight.Right(); // x;
|
||||
float m01 = nRight.Up(); // y;
|
||||
float m02 = nRight.Forward(); // z;
|
||||
float m10 = nUp.Right(); // x;
|
||||
float m11 = nUp.Up(); // y;
|
||||
float m12 = nUp.Forward(); // z;
|
||||
float m20 = nForward.Right(); // x;
|
||||
float m21 = nForward.Up(); // y;
|
||||
float m22 = nForward.Forward(); // z;
|
||||
|
||||
float num8 = (m00 + m11) + m22;
|
||||
Quaternion quaternion = Quaternion();
|
||||
if (num8 > 0) {
|
||||
float num = sqrtf(num8 + 1);
|
||||
quaternion.w = num * 0.5f;
|
||||
num = 0.5f / num;
|
||||
quaternion.x = (m12 - m21) * num;
|
||||
quaternion.y = (m20 - m02) * num;
|
||||
quaternion.z = (m01 - m10) * num;
|
||||
return quaternion;
|
||||
}
|
||||
if ((m00 >= m11) && (m00 >= m22)) {
|
||||
float num7 = sqrtf(((1 + m00) - m11) - m22);
|
||||
float num4 = 0.5F / num7;
|
||||
quaternion.x = 0.5f * num7;
|
||||
quaternion.y = (m01 + m10) * num4;
|
||||
quaternion.z = (m02 + m20) * num4;
|
||||
quaternion.w = (m12 - m21) * num4;
|
||||
return quaternion;
|
||||
}
|
||||
if (m11 > m22) {
|
||||
float num6 = sqrtf(((1 + m11) - m00) - m22);
|
||||
float num3 = 0.5F / num6;
|
||||
quaternion.x = (m10 + m01) * num3;
|
||||
quaternion.y = 0.5F * num6;
|
||||
quaternion.z = (m21 + m12) * num3;
|
||||
quaternion.w = (m20 - m02) * num3;
|
||||
return quaternion;
|
||||
}
|
||||
float num5 = sqrtf(((1 + m22) - m00) - m11);
|
||||
float num2 = 0.5F / num5;
|
||||
quaternion.x = (m20 + m02) * num2;
|
||||
quaternion.y = (m21 + m12) * num2;
|
||||
quaternion.z = 0.5F * num5;
|
||||
quaternion.w = (m01 - m10) * num2;
|
||||
return quaternion;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::FromToRotation(Vector3 fromDirection,
|
||||
Vector3 toDirection) {
|
||||
Vector3 axis = Vector3::Cross(fromDirection, toDirection);
|
||||
axis = Vector3::Normalize(axis);
|
||||
AngleOf<float> angle = Vector3::SignedAngle(fromDirection, toDirection, axis);
|
||||
Quaternion rotation = Quaternion::AngleAxis(angle.InDegrees(), axis);
|
||||
return rotation;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::RotateTowards(const Quaternion& from,
|
||||
const Quaternion& to,
|
||||
float maxDegreesDelta) {
|
||||
float num = Quaternion::Angle(from, to);
|
||||
if (num == 0) {
|
||||
return to;
|
||||
}
|
||||
float t = (float)fmin(1, maxDegreesDelta / num);
|
||||
return SlerpUnclamped(from, to, t);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) {
|
||||
if (Vector3::SqrMagnitude(axis) == 0.0f)
|
||||
return Quaternion();
|
||||
|
||||
Quaternion result = Quaternion();
|
||||
float radians = angle * Deg2Rad;
|
||||
radians *= 0.5f;
|
||||
|
||||
Vector3 axis2 = axis * (float)sin(radians);
|
||||
result.x = axis2.Right(); // x;
|
||||
result.y = axis2.Up(); // y;
|
||||
result.z = axis2.Forward(); // z;
|
||||
result.w = (float)cos(radians);
|
||||
|
||||
return Quaternion::Normalize(result);
|
||||
}
|
||||
|
||||
float Quaternion::Angle(Quaternion a, Quaternion b) {
|
||||
float f = Quaternion::Dot(a, b);
|
||||
return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg;
|
||||
}
|
||||
|
||||
void Quaternion::ToAngleAxis(float* angle, Vector3* axis) {
|
||||
Quaternion::ToAxisAngleRad(*this, axis, angle);
|
||||
*angle *= Rad2Deg;
|
||||
}
|
||||
|
||||
void Quaternion::ToAxisAngleRad(const Quaternion& q,
|
||||
Vector3* const axis,
|
||||
float* angle) {
|
||||
Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q;
|
||||
*angle = 2.0f * acosf(q1.w); // angle
|
||||
float den = sqrtf(1.0F - q1.w * q1.w);
|
||||
if (den > 0.0001f) {
|
||||
*axis = Vector3::Normalize(q1.xyz() / den);
|
||||
} else {
|
||||
// This occurs when the angle is zero.
|
||||
// Not a problem: just set an arbitrary normalized axis.
|
||||
*axis = Vector3(1, 0, 0);
|
||||
}
|
||||
}
|
||||
Quaternion Quaternion::SlerpUnclamped(const Quaternion& a,
|
||||
const Quaternion& b,
|
||||
float t) {
|
||||
// if either input is zero, return the other.
|
||||
if (Quaternion::GetLengthSquared(a) == 0.0f) {
|
||||
if (Quaternion::GetLengthSquared(b) == 0.0f) {
|
||||
return Quaternion();
|
||||
}
|
||||
return b;
|
||||
} else if (Quaternion::GetLengthSquared(b) == 0.0f) {
|
||||
return a;
|
||||
}
|
||||
|
||||
const Vector3 axyz = a.xyz();
|
||||
const Vector3 bxyz = b.xyz();
|
||||
float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz);
|
||||
|
||||
Quaternion b2 = b;
|
||||
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
|
||||
// angle = 0.0f, so just return one input.
|
||||
return a;
|
||||
} else if (cosHalfAngle < 0.0f) {
|
||||
b2.x = -b.x;
|
||||
b2.y = -b.y;
|
||||
b2.z = -b.z;
|
||||
b2.w = -b.w;
|
||||
cosHalfAngle = -cosHalfAngle;
|
||||
}
|
||||
|
||||
float blendA;
|
||||
float blendB;
|
||||
if (cosHalfAngle < 0.99f) {
|
||||
// do proper slerp for big angles
|
||||
float halfAngle = acosf(cosHalfAngle);
|
||||
float sinHalfAngle = sinf(halfAngle);
|
||||
float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
|
||||
blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
|
||||
blendB = sinf(halfAngle * t) * oneOverSinHalfAngle;
|
||||
} else {
|
||||
// do lerp if angle is really small.
|
||||
blendA = 1.0f - t;
|
||||
blendB = t;
|
||||
}
|
||||
Vector3 v = axyz * blendA + b2.xyz() * blendB;
|
||||
Quaternion result =
|
||||
Quaternion(v.Right(), v.Up(), v.Forward(), blendA * a.w + blendB * b2.w);
|
||||
if (result.GetLengthSquared() > 0.0f)
|
||||
return Quaternion::Normalize(result);
|
||||
else
|
||||
return Quaternion();
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Slerp(const Quaternion& a,
|
||||
const Quaternion& b,
|
||||
float t) {
|
||||
if (t > 1)
|
||||
t = 1;
|
||||
if (t < 0)
|
||||
t = 0;
|
||||
return Quaternion::SlerpUnclamped(a, b, t);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Euler(float x, float y, float z) {
|
||||
return Quaternion::Euler(Vector3(x, y, z));
|
||||
}
|
||||
Quaternion Quaternion::Euler(Vector3 euler) {
|
||||
return Quaternion::FromEulerRad(euler * Deg2Rad);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::FromEulerRad(Vector3 euler) {
|
||||
float yaw = euler.Right();
|
||||
float pitch = euler.Up();
|
||||
float roll = euler.Forward();
|
||||
float rollOver2 = roll * 0.5f;
|
||||
float sinRollOver2 = (float)sin((float)rollOver2);
|
||||
float cosRollOver2 = (float)cos((float)rollOver2);
|
||||
float pitchOver2 = pitch * 0.5f;
|
||||
float sinPitchOver2 = (float)sin((float)pitchOver2);
|
||||
float cosPitchOver2 = (float)cos((float)pitchOver2);
|
||||
float yawOver2 = yaw * 0.5f;
|
||||
float sinYawOver2 = (float)sin((float)yawOver2);
|
||||
float cosYawOver2 = (float)cos((float)yawOver2);
|
||||
Quaternion result;
|
||||
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
|
||||
sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
|
||||
cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
|
||||
sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
||||
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
|
||||
sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
||||
return result;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::EulerXYZ(float x, float y, float z) {
|
||||
return Quaternion::EulerXYZ(Vector3(x, y, z));
|
||||
}
|
||||
Quaternion Quaternion::EulerXYZ(Vector3 euler) {
|
||||
return Quaternion::FromEulerRadXYZ(euler * Deg2Rad);
|
||||
}
|
||||
Quaternion Quaternion::FromEulerRadXYZ(Vector3 euler) {
|
||||
float yaw = euler.Right(); // x;
|
||||
float pitch = euler.Up(); // y;
|
||||
float roll = euler.Forward(); // z;
|
||||
float rollOver2 = roll * 0.5f;
|
||||
float sinRollOver2 = (float)sin((float)rollOver2);
|
||||
float cosRollOver2 = (float)cos((float)rollOver2);
|
||||
float pitchOver2 = pitch * 0.5f;
|
||||
float sinPitchOver2 = (float)sin((float)pitchOver2);
|
||||
float cosPitchOver2 = (float)cos((float)pitchOver2);
|
||||
float yawOver2 = yaw * 0.5f;
|
||||
float sinYawOver2 = (float)sin((float)yawOver2);
|
||||
float cosYawOver2 = (float)cos((float)yawOver2);
|
||||
Quaternion result;
|
||||
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
|
||||
sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 -
|
||||
cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 +
|
||||
sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
||||
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
|
||||
sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
||||
return result;
|
||||
}
|
||||
|
||||
float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) {
|
||||
Quaternion secondaryRotation = GetRotationAround(axis, rotation);
|
||||
float rotationAngle;
|
||||
Vector3 rotationAxis;
|
||||
secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis);
|
||||
|
||||
// Do the axis point in opposite directions?
|
||||
if (Vector3::Dot(axis, rotationAxis) < 0)
|
||||
rotationAngle = -rotationAngle;
|
||||
|
||||
return rotationAngle;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) {
|
||||
Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis
|
||||
Vector3 p = Vector3::Project(
|
||||
ra, axis); // return projection ra on to axis (parallel component)
|
||||
Quaternion twist = Quaternion(p.Right(), p.Up(), p.Forward(), rotation.w);
|
||||
twist = Quaternion::Normalize(twist);
|
||||
return twist;
|
||||
}
|
||||
|
||||
void Quaternion::GetSwingTwist(Vector3 axis,
|
||||
Quaternion rotation,
|
||||
Quaternion* swing,
|
||||
Quaternion* twist) {
|
||||
*twist = GetRotationAround(axis, rotation);
|
||||
*swing = rotation * Quaternion::Inverse(*twist);
|
||||
}
|
||||
@ -1,293 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef QUATERNION_H
|
||||
#define QUATERNION_H
|
||||
|
||||
#include "Vector3.h"
|
||||
|
||||
extern "C" {
|
||||
/// <summary>
|
||||
/// A quaternion (C-style)
|
||||
/// </summary>
|
||||
/// This is a C-style implementation
|
||||
typedef struct Quat {
|
||||
/// <summary>
|
||||
/// The x component
|
||||
/// </summary>
|
||||
float x;
|
||||
/// <summary>
|
||||
/// The y component
|
||||
/// </summary>
|
||||
float y;
|
||||
/// <summary>
|
||||
/// The z component
|
||||
/// </summary>
|
||||
float z;
|
||||
/// <summary>
|
||||
/// The w component
|
||||
/// </summary>
|
||||
float w;
|
||||
} Quat;
|
||||
}
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
/// <summary>
|
||||
/// A quaternion
|
||||
/// </summary>
|
||||
struct Quaternion : Quat {
|
||||
public:
|
||||
/// <summary>
|
||||
/// Create a new identity quaternion
|
||||
/// </summary>
|
||||
Quaternion();
|
||||
/// <summary>
|
||||
/// create a new quaternion with the given values
|
||||
/// </summary>
|
||||
/// <param name="_x">x component</param>
|
||||
/// <param name="_y">y component</param>
|
||||
/// <param name="_z">z component</param>
|
||||
/// <param name="_w">w component</param>
|
||||
Quaternion(float _x, float _y, float _z, float _w);
|
||||
/// <summary>
|
||||
/// Create a quaternion from C-style Quat
|
||||
/// </summary>
|
||||
/// <param name="q"></param>
|
||||
Quaternion(Quat q);
|
||||
/// <summary>
|
||||
/// Quaternion destructor
|
||||
/// </summary>
|
||||
~Quaternion();
|
||||
|
||||
/// <summary>
|
||||
/// An identity quaternion
|
||||
/// </summary>
|
||||
const static Quaternion identity;
|
||||
|
||||
/// <summary>
|
||||
/// Convert to unit quaternion
|
||||
/// </summary>
|
||||
/// This will preserve the orientation,
|
||||
/// but ensures that it is a unit quaternion.
|
||||
void Normalize();
|
||||
/// <summary>
|
||||
/// Convert to unity quaternion
|
||||
/// </summary>
|
||||
/// <param name="q">The quaternion to convert</param>
|
||||
/// <returns>A unit quaternion</returns>
|
||||
/// This will preserve the orientation,
|
||||
/// but ensures that it is a unit quaternion.
|
||||
static Quaternion Normalize(const Quaternion& q);
|
||||
|
||||
/// <summary>
|
||||
/// Convert to euler angles
|
||||
/// </summary>
|
||||
/// <param name="q">The quaternion to convert</param>
|
||||
/// <returns>A vector containing euler angles</returns>
|
||||
/// The euler angles performed in the order: Z, X, Y
|
||||
static Vector3 ToAngles(const Quaternion& q);
|
||||
|
||||
/// <summary>
|
||||
/// Rotate a vector using this quaterion
|
||||
/// </summary>
|
||||
/// <param name="vector">The vector to rotate</param>
|
||||
/// <returns>The rotated vector</returns>
|
||||
Vector3 operator*(const Vector3& vector) const;
|
||||
/// <summary>
|
||||
/// Multiply this quaternion with another quaternion
|
||||
/// </summary>
|
||||
/// <param name="rotation">The quaternion to multiply with</param>
|
||||
/// <returns>The resulting rotation</returns>
|
||||
/// The result will be this quaternion rotated according to
|
||||
/// the give rotation.
|
||||
Quaternion operator*(const Quaternion& rotation) const;
|
||||
|
||||
/// <summary>
|
||||
/// Check the equality of two quaternions
|
||||
/// </summary>
|
||||
/// <param name="quaternion">The quaternion to compare to</param>
|
||||
/// <returns>True when the components of the quaternions are
|
||||
/// identical</returns> Note that this does not compare the rotations
|
||||
/// themselves. Two quaternions with the same rotational effect may have
|
||||
/// different components. Use Quaternion::Angle to check if the rotations are
|
||||
/// the same.
|
||||
bool operator==(const Quaternion& quaternion) const;
|
||||
|
||||
/// <summary>
|
||||
/// The inverse of quaterion
|
||||
/// </summary>
|
||||
/// <param name="quaternion">The quaternion for which the inverse is
|
||||
/// needed</param> <returns>The inverted quaternion</returns>
|
||||
static Quaternion Inverse(Quaternion quaternion);
|
||||
|
||||
/// <summary>
|
||||
/// A rotation which looks in the given direction
|
||||
/// </summary>
|
||||
/// <param name="forward">The look direction</param>
|
||||
/// <param name="upwards">The up direction</param>
|
||||
/// <returns>The look rotation</returns>
|
||||
static Quaternion LookRotation(const Vector3& forward,
|
||||
const Vector3& upwards);
|
||||
/// <summary>
|
||||
/// Creates a quaternion with the given forward direction with up =
|
||||
/// Vector3::up
|
||||
/// </summary>
|
||||
/// <param name="forward">The look direction</param>
|
||||
/// <returns>The rotation for this direction</returns>
|
||||
/// For the rotation, Vector::up is used for the up direction.
|
||||
/// Note: if the forward direction == Vector3::up, the result is
|
||||
/// Quaternion::identity
|
||||
static Quaternion LookRotation(const Vector3& forward);
|
||||
|
||||
/// <summary>
|
||||
/// Calculat the rotation from on vector to another
|
||||
/// </summary>
|
||||
/// <param name="fromDirection">The from direction</param>
|
||||
/// <param name="toDirection">The to direction</param>
|
||||
/// <returns>The rotation from the first to the second vector</returns>
|
||||
static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection);
|
||||
|
||||
/// <summary>
|
||||
/// Rotate form one orientation to anther with a maximum amount of degrees
|
||||
/// </summary>
|
||||
/// <param name="from">The from rotation</param>
|
||||
/// <param name="to">The destination rotation</param>
|
||||
/// <param name="maxDegreesDelta">The maximum amount of degrees to
|
||||
/// rotate</param> <returns>The possibly limited rotation</returns>
|
||||
static Quaternion RotateTowards(const Quaternion& from,
|
||||
const Quaternion& to,
|
||||
float maxDegreesDelta);
|
||||
|
||||
/// <summary>
|
||||
/// Convert an angle/axis representation to a quaternion
|
||||
/// </summary>
|
||||
/// <param name="angle">The angle</param>
|
||||
/// <param name="axis">The axis</param>
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
static Quaternion AngleAxis(float angle, const Vector3& axis);
|
||||
/// <summary>
|
||||
/// Convert this quaternion to angle/axis representation
|
||||
/// </summary>
|
||||
/// <param name="angle">A pointer to the angle for the result</param>
|
||||
/// <param name="axis">A pointer to the axis for the result</param>
|
||||
void ToAngleAxis(float* angle, Vector3* axis);
|
||||
|
||||
/// <summary>
|
||||
/// Get the angle between two orientations
|
||||
/// </summary>
|
||||
/// <param name="orientation1">The first orientation</param>
|
||||
/// <param name="orientation2">The second orientation</param>
|
||||
/// <returns>The smallest angle in degrees between the two
|
||||
/// orientations</returns>
|
||||
static float Angle(Quaternion orientation1, Quaternion orientation2);
|
||||
/// <summary>
|
||||
/// Sherical lerp between two rotations
|
||||
/// </summary>
|
||||
/// <param name="rotation1">The first rotation</param>
|
||||
/// <param name="rotation2">The second rotation</param>
|
||||
/// <param name="factor">The factor between 0 and 1.</param>
|
||||
/// <returns>The resulting rotation</returns>
|
||||
/// A factor 0 returns rotation1, factor1 returns rotation2.
|
||||
static Quaternion Slerp(const Quaternion& rotation1,
|
||||
const Quaternion& rotation2,
|
||||
float factor);
|
||||
/// <summary>
|
||||
/// Unclamped sherical lerp between two rotations
|
||||
/// </summary>
|
||||
/// <param name="rotation1">The first rotation</param>
|
||||
/// <param name="rotation2">The second rotation</param>
|
||||
/// <param name="factor">The factor</param>
|
||||
/// <returns>The resulting rotation</returns>
|
||||
/// A factor 0 returns rotation1, factor1 returns rotation2.
|
||||
/// Values outside the 0..1 range will result in extrapolated rotations
|
||||
static Quaternion SlerpUnclamped(const Quaternion& rotation1,
|
||||
const Quaternion& rotation2,
|
||||
float factor);
|
||||
|
||||
/// <summary>
|
||||
/// Create a rotation from euler angles
|
||||
/// </summary>
|
||||
/// <param name="x">The angle around the right axis</param>
|
||||
/// <param name="y">The angle around the upward axis</param>
|
||||
/// <param name="z">The angle around the forward axis</param>
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
/// Rotation are appied in the order Z, X, Y.
|
||||
static Quaternion Euler(float x, float y, float z);
|
||||
/// <summary>
|
||||
/// Create a rotation from a vector containing euler angles
|
||||
/// </summary>
|
||||
/// <param name="eulerAngles">Vector with the euler angles</param>
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
/// Rotation are appied in the order Z, X, Y.
|
||||
static Quaternion Euler(Vector3 eulerAngles);
|
||||
|
||||
/// <summary>
|
||||
/// Create a rotation from euler angles
|
||||
/// </summary>
|
||||
/// <param name="x">The angle around the right axis</param>
|
||||
/// <param name="y">The angle around the upward axis</param>
|
||||
/// <param name="z">The angle around the forward axis</param>
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
/// Rotation are appied in the order X, Y, Z.
|
||||
static Quaternion EulerXYZ(float x, float y, float z);
|
||||
/// <summary>
|
||||
/// Create a rotation from a vector containing euler angles
|
||||
/// </summary>
|
||||
/// <param name="eulerAngles">Vector with the euler angles</param>
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
/// Rotation are appied in the order X, Y, Z.
|
||||
static Quaternion EulerXYZ(Vector3 eulerAngles);
|
||||
|
||||
/// <summary>
|
||||
/// Returns the angle of around the give axis for a rotation
|
||||
/// </summary>
|
||||
/// <param name="axis">The axis around which the angle should be
|
||||
/// computed</param> <param name="rotation">The source rotation</param>
|
||||
/// <returns>The signed angle around the axis</returns>
|
||||
static float GetAngleAround(Vector3 axis, Quaternion rotation);
|
||||
/// <summary>
|
||||
/// Returns the rotation limited around the given axis
|
||||
/// </summary>
|
||||
/// <param name="axis">The axis which which the rotation should be
|
||||
/// limited</param> <param name="rotation">The source rotation</param>
|
||||
/// <returns>The rotation around the given axis</returns>
|
||||
static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation);
|
||||
/// <summary>
|
||||
/// Swing-twist decomposition of a rotation
|
||||
/// </summary>
|
||||
/// <param name="axis">The base direction for the decomposition</param>
|
||||
/// <param name="rotation">The source rotation</param>
|
||||
/// <param name="swing">A pointer to the quaternion for the swing
|
||||
/// result</param> <param name="twist">A pointer to the quaternion for the
|
||||
/// twist result</param>
|
||||
static void GetSwingTwist(Vector3 axis,
|
||||
Quaternion rotation,
|
||||
Quaternion* swing,
|
||||
Quaternion* twist);
|
||||
|
||||
/// <summary>
|
||||
/// Calculate the dot product of two quaternions
|
||||
/// </summary>
|
||||
/// <param name="rotation1">The first rotation</param>
|
||||
/// <param name="rotation2">The second rotation</param>
|
||||
/// <returns></returns>
|
||||
static float Dot(Quaternion rotation1, Quaternion rotation2);
|
||||
|
||||
private:
|
||||
float GetLength() const;
|
||||
float GetLengthSquared() const;
|
||||
static float GetLengthSquared(const Quaternion& q);
|
||||
|
||||
void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
|
||||
static Quaternion FromEulerRad(Vector3 euler);
|
||||
static Quaternion FromEulerRadXYZ(Vector3 euler);
|
||||
|
||||
Vector3 xyz() const;
|
||||
};
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#endif
|
||||
@ -1,21 +0,0 @@
|
||||
\mainpage Linear Algebra
|
||||
|
||||
Linear algebra library
|
||||
|
||||
Main components
|
||||
---------------
|
||||
Carthesian coordinate systems
|
||||
* [Vector3](#Passer::LinearAlgebra::Vector3): A 3D carthesian vector
|
||||
* [Vector2](#Passer::LinearAlgebra::Vector2): A 2D carthesian vector
|
||||
|
||||
Other coodinate systems
|
||||
* [Polar](#Passer::LinearAlgebra::PolarOf): A 2D polar vector
|
||||
* [Spherical](#Passer::LinearAlgebra::SphericalOf): A 3D spherical vector
|
||||
|
||||
Rotations
|
||||
* [Quaternion](#Passer::LinearAlgebra::Quaternion): A quaternion rotation
|
||||
* [SwingTwist](#Passer::LinearAlgebra::SwingTwistOf): A swing/twist angular rotation
|
||||
|
||||
Basics
|
||||
* [Angle](#Passer::LinearAlgebra::AngleOf): An angle
|
||||
* [Direction](#Passer::LinearAlgebra::DirectionOf): A direction using angles
|
||||
@ -1,303 +0,0 @@
|
||||
#include "Spherical.h"
|
||||
|
||||
#include "Angle.h"
|
||||
#include "Quaternion.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T>::SphericalOf() {
|
||||
this->distance = 0.0f;
|
||||
this->direction = DirectionOf<T>();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T>::SphericalOf(float distance,
|
||||
AngleOf<T> horizontal,
|
||||
AngleOf<T> vertical) {
|
||||
if (distance < 0) {
|
||||
this->distance = -distance;
|
||||
this->direction = -DirectionOf<T>(horizontal, vertical);
|
||||
} else {
|
||||
this->distance = distance;
|
||||
this->direction = DirectionOf<T>(horizontal, vertical);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
|
||||
if (distance < 0) {
|
||||
this->distance = -distance;
|
||||
this->direction = -direction;
|
||||
} else {
|
||||
this->distance = distance;
|
||||
this->direction = direction;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::Degrees(float distance,
|
||||
float horizontal,
|
||||
float vertical) {
|
||||
AngleOf<T> horizontalAngle = AngleOf<T>::Degrees(horizontal);
|
||||
AngleOf<T> verticalAngle = AngleOf<T>::Degrees(vertical);
|
||||
SphericalOf<T> r = SphericalOf<T>(distance, horizontalAngle, verticalAngle);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::Radians(float distance,
|
||||
float horizontal,
|
||||
float vertical) {
|
||||
return SphericalOf<T>(distance, AngleOf<T>::Radians(horizontal),
|
||||
AngleOf<T>::Radians(vertical));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
|
||||
AngleOf<T> horizontal = polar.angle;
|
||||
AngleOf<T> vertical = AngleOf<T>();
|
||||
SphericalOf<T> r = SphericalOf(polar.distance, horizontal, vertical);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
|
||||
float distance = v.magnitude();
|
||||
if (distance == 0.0f) {
|
||||
return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
|
||||
} else {
|
||||
AngleOf<T> verticalAngle =
|
||||
AngleOf<T>::Radians((pi / 2 - acosf(v.Up() / distance)));
|
||||
AngleOf<T> horizontalAngle =
|
||||
AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
|
||||
return SphericalOf(distance, horizontalAngle, verticalAngle);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Converts spherical coordinates to a 3D vector.
|
||||
*
|
||||
* This function converts the spherical coordinates represented by the
|
||||
* SphericalOf object to a 3D vector (Vector3). The conversion is based
|
||||
* on the distance and direction (vertical and horizontal angles) of the
|
||||
* spherical coordinates.
|
||||
*
|
||||
* @tparam T The type of the distance and direction values.
|
||||
* @return Vector3 The 3D vector representation of the spherical coordinates.
|
||||
*/
|
||||
template <typename T>
|
||||
Vector3 SphericalOf<T>::ToVector3() const {
|
||||
float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
|
||||
float horizontalRad = this->direction.horizontal.InRadians();
|
||||
|
||||
float cosVertical = cosf(verticalRad);
|
||||
float sinVertical = sinf(verticalRad);
|
||||
float cosHorizontal = cosf(horizontalRad);
|
||||
float sinHorizontal = sinf(horizontalRad);
|
||||
|
||||
float x = this->distance * sinVertical * sinHorizontal;
|
||||
float y = this->distance * cosVertical;
|
||||
float z = this->distance * sinVertical * cosHorizontal;
|
||||
|
||||
Vector3 v = Vector3(x, y, z);
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::zero =
|
||||
SphericalOf<T>(0.0f, AngleOf<T>(), AngleOf<T>());
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::forward =
|
||||
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>());
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::back =
|
||||
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(180), AngleOf<T>());
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::right =
|
||||
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(90), AngleOf<T>());
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::left =
|
||||
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(-90), AngleOf<T>());
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::up =
|
||||
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(90));
|
||||
template <typename T>
|
||||
const SphericalOf<T> SphericalOf<T>::down =
|
||||
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(-90));
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
|
||||
SphericalOf<T> v = SphericalOf<T>(distance, this->direction);
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator-() const {
|
||||
SphericalOf<T> v = SphericalOf<T>(
|
||||
this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
|
||||
this->direction.vertical + AngleOf<T>::Degrees(180));
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
|
||||
// let's do it the easy way...
|
||||
Vector3 v1 = this->ToVector3();
|
||||
Vector3 v2 = s2.ToVector3();
|
||||
Vector3 v = v1 - v2;
|
||||
SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
|
||||
return r;
|
||||
}
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
|
||||
*this = *this - v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
|
||||
// let's do it the easy way...
|
||||
Vector3 v1 = this->ToVector3();
|
||||
Vector3 v2 = s2.ToVector3();
|
||||
Vector3 v = v1 + v2;
|
||||
SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
|
||||
return r;
|
||||
/*
|
||||
// This is the hard way...
|
||||
if (v2.distance <= 0)
|
||||
return Spherical(this->distance, this->horizontalAngle,
|
||||
this->verticalAngle);
|
||||
if (this->distance <= 0)
|
||||
return v2;
|
||||
|
||||
float deltaHorizontalAngle =
|
||||
(float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
|
||||
float horizontalRotation = deltaHorizontalAngle < 0
|
||||
? 180 + deltaHorizontalAngle
|
||||
: 180 - deltaHorizontalAngle;
|
||||
float deltaVerticalAngle =
|
||||
Angle::Normalize(v2.verticalAngle - this->verticalAngle);
|
||||
float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
|
||||
: 180 - deltaVerticalAngle;
|
||||
|
||||
if (horizontalRotation == 180 && verticalRotation == 180)
|
||||
// angle is too small, take this angle and add the distances
|
||||
return Spherical(this->distance + v2.distance, this->horizontalAngle,
|
||||
this->verticalAngle);
|
||||
|
||||
Angle rotation = AngleBetween(*this, v2);
|
||||
float newDistance =
|
||||
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
|
||||
float angle =
|
||||
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
|
||||
|
||||
// Now we have to project the angle to the horizontal and vertical planes...
|
||||
// The axis for the angle is the cross product of the two spherical vectors
|
||||
// (which function we do not have either...)
|
||||
float horizontalAngle = 0;
|
||||
float verticalAngle = 0;
|
||||
|
||||
float newHorizontalAngle =
|
||||
deltaHorizontalAngle < 0
|
||||
? Angle::Normalize(this->horizontalAngle - horizontalAngle)
|
||||
: Angle::Normalize(this->horizontalAngle + horizontalAngle);
|
||||
float newVerticalAngle =
|
||||
deltaVerticalAngle < 0
|
||||
? Angle::Normalize(this->verticalAngle - verticalAngle)
|
||||
: Angle::Normalize(this->verticalAngle + verticalAngle);
|
||||
|
||||
Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
|
||||
*/
|
||||
}
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
|
||||
*this = *this + v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator*=(float f) {
|
||||
this->distance *= f;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::operator/=(float f) {
|
||||
this->distance /= f;
|
||||
return *this;
|
||||
}
|
||||
|
||||
#include "FloatSingle.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
const float epsilon = 1E-05f;
|
||||
|
||||
template <typename T>
|
||||
float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
|
||||
const SphericalOf<T>& v2) {
|
||||
// SphericalOf<T> difference = v1 - v2;
|
||||
// return difference.distance;
|
||||
Vector3 vec1 = v1.ToVector3();
|
||||
Vector3 vec2 = v2.ToVector3();
|
||||
float distance = Vector3::Distance(vec1, vec2);
|
||||
return distance;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
|
||||
const SphericalOf& v2) {
|
||||
// float denominator = v1.distance * v2.distance;
|
||||
// if (denominator < epsilon)
|
||||
// return 0.0f;
|
||||
|
||||
Vector3 v1_3 = v1.ToVector3();
|
||||
Vector3 v2_3 = v2.ToVector3();
|
||||
// float dot = Vector3::Dot(v1_3, v2_3);
|
||||
// float fraction = dot / denominator;
|
||||
// if (isnan(fraction))
|
||||
// return fraction; // short cut to returning NaN universally
|
||||
|
||||
// float cdot = Float::Clamp(fraction, -1.0, 1.0);
|
||||
// float r = ((float)acos(cdot)) * Rad2Deg;
|
||||
AngleSingle r = Vector3::Angle(v1_3, v2_3);
|
||||
return AngleOf<T>::Degrees(r.InDegrees());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
|
||||
const SphericalOf<T>& v2,
|
||||
const SphericalOf<T>& axis) {
|
||||
Vector3 v1_vector = v1.ToVector3();
|
||||
Vector3 v2_vector = v2.ToVector3();
|
||||
Vector3 axis_vector = axis.ToVector3();
|
||||
AngleSingle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector);
|
||||
return AngleOf<T>::Degrees(r.InDegrees());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
|
||||
AngleOf<T> horizontalAngle,
|
||||
AngleOf<T> verticalAngle) {
|
||||
SphericalOf<T> r =
|
||||
SphericalOf(v.distance, v.direction.horizontal + horizontalAngle,
|
||||
v.direction.vertical + verticalAngle);
|
||||
return r;
|
||||
}
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
|
||||
AngleOf<T> a) {
|
||||
SphericalOf<T> r =
|
||||
SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical);
|
||||
return r;
|
||||
}
|
||||
template <typename T>
|
||||
SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
|
||||
AngleOf<T> a) {
|
||||
SphericalOf<T> r =
|
||||
SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);
|
||||
return r;
|
||||
}
|
||||
|
||||
template class SphericalOf<float>;
|
||||
template class SphericalOf<signed short>;
|
||||
@ -1,194 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef SPHERICAL_H
|
||||
#define SPHERICAL_H
|
||||
|
||||
#include "Direction.h"
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
struct Vector3;
|
||||
template <typename T>
|
||||
class PolarOf;
|
||||
|
||||
/// @brief A spherical vector using angles in various representations
|
||||
/// @tparam T The implementation type used for the representations of the agles
|
||||
template <typename T>
|
||||
class SphericalOf {
|
||||
public:
|
||||
/// @brief The distance in meters
|
||||
/// @remark The distance should never be negative
|
||||
float distance;
|
||||
/// @brief The direction of the vector
|
||||
DirectionOf<T> direction;
|
||||
|
||||
SphericalOf<T>();
|
||||
SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
|
||||
SphericalOf<T>(float distance, DirectionOf<T> direction);
|
||||
|
||||
/// @brief Create spherical vector without using AngleOf type. All given
|
||||
/// angles are in degrees
|
||||
/// @param distance The distance in meters
|
||||
/// @param horizontal The horizontal angle in degrees
|
||||
/// @param vertical The vertical angle in degrees
|
||||
/// @return The spherical vector
|
||||
static SphericalOf<T> Degrees(float distance,
|
||||
float horizontal,
|
||||
float vertical);
|
||||
/// @brief Short-hand Deg alias for the Degrees function
|
||||
constexpr static auto Deg = Degrees;
|
||||
/// @brief Create sperical vector without using the AngleOf type. All given
|
||||
/// angles are in radians
|
||||
/// @param distance The distance in meters
|
||||
/// @param horizontal The horizontal angle in radians
|
||||
/// @param vertical The vertical angle in radians
|
||||
/// @return The spherical vectpr
|
||||
static SphericalOf<T> Radians(float distance,
|
||||
float horizontal,
|
||||
float vertical);
|
||||
// Short-hand Rad alias for the Radians function
|
||||
constexpr static auto Rad = Radians;
|
||||
|
||||
/// @brief Create a Spherical coordinate from a Polar coordinate
|
||||
/// @param v The polar coordinate
|
||||
/// @return The spherical coordinate with the vertical angle set to zero.
|
||||
static SphericalOf<T> FromPolar(PolarOf<T> v);
|
||||
|
||||
/// @brief Create a Spherical coordinate from a Vector3 coordinate
|
||||
/// @param v The vector coordinate
|
||||
/// @return The spherical coordinate
|
||||
static SphericalOf<T> FromVector3(Vector3 v);
|
||||
/// @brief Convert the spherical coordinate to a Vector3 coordinate
|
||||
/// @return The vector coordinate
|
||||
Vector3 ToVector3() const;
|
||||
|
||||
/// @brief A spherical vector with zero degree angles and distance
|
||||
const static SphericalOf<T> zero;
|
||||
/// @brief A normalized forward-oriented vector
|
||||
const static SphericalOf<T> forward;
|
||||
/// @brief A normalized back-oriented vector
|
||||
const static SphericalOf<T> back;
|
||||
/// @brief A normalized right-oriented vector
|
||||
const static SphericalOf<T> right;
|
||||
/// @brief A normalized left-oriented vector
|
||||
const static SphericalOf<T> left;
|
||||
/// @brief A normalized up-oriented vector
|
||||
const static SphericalOf<T> up;
|
||||
/// @brief A normalized down-oriented vector
|
||||
const static SphericalOf<T> down;
|
||||
|
||||
/// @brief Update the distance component of the spherical coordinate
|
||||
/// @param distance The new distance
|
||||
/// @return The updated coordinate
|
||||
SphericalOf<T> WithDistance(float distance);
|
||||
|
||||
/// @brief Negate the vector
|
||||
/// @return The negated vector
|
||||
/// This will rotate the vector by 180 degrees horizontally and
|
||||
/// vertically. Distance will stay the same.
|
||||
SphericalOf<T> operator-() const;
|
||||
|
||||
/// @brief Subtract a spherical vector from this vector
|
||||
/// @param v The vector to subtract
|
||||
/// @return The result of the subtraction
|
||||
SphericalOf<T> operator-(const SphericalOf<T>& v) const;
|
||||
SphericalOf<T> operator-=(const SphericalOf<T>& v);
|
||||
/// @brief Add a spherical vector to this vector
|
||||
/// @param v The vector to add
|
||||
/// @return The result of the addition
|
||||
SphericalOf<T> operator+(const SphericalOf<T>& v) const;
|
||||
SphericalOf<T> operator+=(const SphericalOf<T>& v);
|
||||
|
||||
/// @brief Scale the vector uniformly up
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled vector
|
||||
/// @remark This operation will scale the distance of the vector. The angle
|
||||
/// will be unaffected.
|
||||
friend SphericalOf<T> operator*(const SphericalOf<T>& v, float f) {
|
||||
return SphericalOf<T>(v.distance * f, v.direction);
|
||||
}
|
||||
friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) {
|
||||
return SphericalOf<T>(f * v.distance, v.direction);
|
||||
}
|
||||
SphericalOf<T> operator*=(float f);
|
||||
/// @brief Scale the vector uniformly down
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled factor
|
||||
/// @remark This operation will scale the distance of the vector. The angle
|
||||
/// will be unaffected.
|
||||
friend SphericalOf<T> operator/(const SphericalOf<T>& v, float f) {
|
||||
return SphericalOf<T>(v.distance / f, v.direction);
|
||||
}
|
||||
friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) {
|
||||
return SphericalOf<T>(f / v.distance, v.direction);
|
||||
}
|
||||
SphericalOf<T> operator/=(float f);
|
||||
|
||||
/// @brief Calculate the distance between two spherical coordinates
|
||||
/// @param v1 The first coordinate
|
||||
/// @param v2 The second coordinate
|
||||
/// @return The distance between the coordinates in meters
|
||||
static float DistanceBetween(const SphericalOf<T>& v1,
|
||||
const SphericalOf<T>& v2);
|
||||
/// @brief Calculate the unsigned angle between two spherical vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The unsigned angle between the vectors
|
||||
static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
|
||||
const SphericalOf<T>& v2);
|
||||
/// @brief Calculate the signed angle between two spherical vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @param axis The axis are which the angle is calculated
|
||||
/// @return The signed angle between the vectors
|
||||
static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
|
||||
const SphericalOf<T>& v2,
|
||||
const SphericalOf<T>& axis);
|
||||
|
||||
/// @brief Rotate a spherical vector
|
||||
/// @param v The vector to rotate
|
||||
/// @param horizontalAngle The horizontal rotation angle in local space
|
||||
/// @param verticalAngle The vertical rotation angle in local space
|
||||
/// @return The rotated vector
|
||||
static SphericalOf<T> Rotate(const SphericalOf& v,
|
||||
AngleOf<T> horizontalAngle,
|
||||
AngleOf<T> verticalAngle);
|
||||
/// @brief Rotate a spherical vector horizontally
|
||||
/// @param v The vector to rotate
|
||||
/// @param angle The horizontal rotation angle in local space
|
||||
/// @return The rotated vector
|
||||
static SphericalOf<T> RotateHorizontal(const SphericalOf<T>& v,
|
||||
AngleOf<T> angle);
|
||||
/// @brief Rotate a spherical vector vertically
|
||||
/// @param v The vector to rotate
|
||||
/// @param angle The vertical rotation angle in local space
|
||||
/// @return The rotated vector
|
||||
static SphericalOf<T> RotateVertical(const SphericalOf<T>& v,
|
||||
AngleOf<T> angle);
|
||||
};
|
||||
|
||||
/// @brief Shorthand notation for a spherical vector using single precision
|
||||
/// floats for the angles This is the fastest implementation on devices with
|
||||
/// floating point harware
|
||||
using SphericalSingle = SphericalOf<float>;
|
||||
/// @brief Shorthand notation for a spherical vector using signed 16-bit words
|
||||
/// for the angles
|
||||
/// @note This is the fastest implementation on devices without floating point
|
||||
/// hardware
|
||||
using Spherical16 = SphericalOf<signed short>;
|
||||
|
||||
#if defined(ARDUINO)
|
||||
using Spherical = Spherical16;
|
||||
#else
|
||||
using Spherical = SphericalSingle;
|
||||
#endif
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#include "Polar.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
#endif
|
||||
@ -1,168 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "SwingTwist.h"
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T>::SwingTwistOf() {
|
||||
this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
|
||||
this->twist = AngleOf<T>();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist) {
|
||||
// Normalize angles
|
||||
AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
|
||||
AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
|
||||
|
||||
if (swing.vertical > deg90 || swing.vertical < -deg90) {
|
||||
swing.horizontal += deg180;
|
||||
swing.vertical = deg180 - swing.vertical;
|
||||
twist += deg180;
|
||||
}
|
||||
|
||||
this->swing = swing;
|
||||
this->twist = twist;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
|
||||
AngleOf<T> vertical,
|
||||
AngleOf<T> twist) {
|
||||
// Normalize angles
|
||||
AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
|
||||
AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
|
||||
|
||||
if (vertical > deg90 || vertical < -deg90) {
|
||||
horizontal += deg180;
|
||||
vertical = deg180 - vertical;
|
||||
twist += deg180;
|
||||
}
|
||||
|
||||
this->swing = DirectionOf<T>(horizontal, vertical);
|
||||
this->twist = twist;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
|
||||
float vertical,
|
||||
float twist) {
|
||||
SwingTwistOf<T> orientation = SwingTwistOf<T>(AngleOf<T>::Degrees(horizontal),
|
||||
-AngleOf<T>::Degrees(vertical),
|
||||
AngleOf<T>::Degrees(twist));
|
||||
// DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
|
||||
// AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
|
||||
// SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
|
||||
return orientation;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
Quaternion SwingTwistOf<T>::ToQuaternion() const {
|
||||
Quaternion q = Quaternion::Euler(-this->swing.vertical.InDegrees(),
|
||||
this->swing.horizontal.InDegrees(),
|
||||
this->twist.InDegrees());
|
||||
return q;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::FromQuaternion(Quaternion q) {
|
||||
Vector3 angles = Quaternion::ToAngles(q);
|
||||
SwingTwistOf<T> r =
|
||||
SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
|
||||
r.Normalize();
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SwingTwistOf<T>::ToAngleAxis() const {
|
||||
Quaternion q = this->ToQuaternion();
|
||||
float angle;
|
||||
Vector3 axis;
|
||||
q.ToAngleAxis(&angle, &axis);
|
||||
DirectionOf<T> direction = DirectionOf<T>::FromVector3(axis);
|
||||
|
||||
SphericalOf<T> aa = SphericalOf<T>(angle, direction);
|
||||
return aa;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::FromAngleAxis(SphericalOf<T> aa) {
|
||||
Vector3 vectorAxis = aa.direction.ToVector3();
|
||||
Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
|
||||
return SwingTwistOf<T>();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool SwingTwistOf<T>::operator==(const SwingTwistOf<T> s) const {
|
||||
return (this->swing == s.swing) && (this->twist == s.twist);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();
|
||||
|
||||
template <typename T>
|
||||
SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
|
||||
SphericalOf<T> v = SphericalOf<T>(
|
||||
vector.distance, vector.direction.horizontal + this->swing.horizontal,
|
||||
vector.direction.vertical + this->swing.vertical);
|
||||
return v;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::operator*(
|
||||
const SwingTwistOf<T>& rotation) const {
|
||||
SwingTwistOf<T> r =
|
||||
SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal,
|
||||
this->swing.vertical + rotation.swing.vertical,
|
||||
this->twist + rotation.twist);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
|
||||
this->swing.horizontal += rotation.swing.horizontal;
|
||||
this->swing.vertical += rotation.swing.vertical;
|
||||
this->twist += rotation.twist;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::Inverse(SwingTwistOf<T> rotation) {
|
||||
SwingTwistOf<T> r = SwingTwistOf<T>(
|
||||
-rotation.swing.horizontal, -rotation.swing.vertical, -rotation.twist);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SwingTwistOf<T> SwingTwistOf<T>::AngleAxis(float angle,
|
||||
const DirectionOf<T>& axis) {
|
||||
Vector3 axis_vector = axis.ToVector3();
|
||||
Quaternion q = Quaternion::AngleAxis(angle, axis_vector);
|
||||
SwingTwistOf<T> r = SwingTwistOf<T>::FromQuaternion(q);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
AngleOf<T> SwingTwistOf<T>::Angle(const SwingTwistOf<T>& r1,
|
||||
const SwingTwistOf<T>& r2) {
|
||||
Quaternion q1 = r1.ToQuaternion();
|
||||
Quaternion q2 = r2.ToQuaternion();
|
||||
float angle = Quaternion::Angle(q1, q2);
|
||||
return AngleOf<T>::Degrees(angle);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void SwingTwistOf<T>::Normalize() {
|
||||
AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
|
||||
AngleOf<T> deg180 = AngleOf<T>::Degrees(180);
|
||||
|
||||
if (this->swing.vertical > deg90 || this->swing.vertical < -deg90) {
|
||||
this->swing.horizontal += deg180;
|
||||
this->swing.vertical = deg180 - this->swing.vertical;
|
||||
this->twist += deg180;
|
||||
}
|
||||
}
|
||||
|
||||
template class SwingTwistOf<float>;
|
||||
template class SwingTwistOf<signed short>;
|
||||
@ -1,85 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef SWINGTWIST_H
|
||||
#define SWINGTWIST_H
|
||||
|
||||
#include "Angle.h"
|
||||
#include "Direction.h"
|
||||
#include "Quaternion.h"
|
||||
#include "Spherical.h"
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
/// @brief An orientation using swing and twist angles in various
|
||||
/// representations
|
||||
/// @tparam T The implmentation type used for the representation of the angles
|
||||
template <typename T>
|
||||
class SwingTwistOf {
|
||||
public:
|
||||
DirectionOf<T> swing;
|
||||
AngleOf<T> twist;
|
||||
|
||||
SwingTwistOf<T>();
|
||||
SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
|
||||
SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
|
||||
|
||||
static SwingTwistOf<T> Degrees(float horizontal,
|
||||
float vertical = 0,
|
||||
float twist = 0);
|
||||
|
||||
Quaternion ToQuaternion() const;
|
||||
static SwingTwistOf<T> FromQuaternion(Quaternion q);
|
||||
|
||||
SphericalOf<T> ToAngleAxis() const;
|
||||
static SwingTwistOf<T> FromAngleAxis(SphericalOf<T> aa);
|
||||
|
||||
const static SwingTwistOf<T> identity;
|
||||
|
||||
bool operator==(const SwingTwistOf<T> d) const;
|
||||
|
||||
/// <summary>
|
||||
/// Rotate a vector using this rotation
|
||||
/// </summary>
|
||||
/// <param name="vector">The vector to rotate</param>
|
||||
/// <returns>The rotated vector</returns>
|
||||
SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
|
||||
/// <summary>
|
||||
/// Multiply this rotation with another rotation
|
||||
/// </summary>
|
||||
/// <param name="rotation">The swing/twist rotation to multiply with</param>
|
||||
/// <returns>The resulting swing/twist rotation</returns>
|
||||
/// The result will be this rotation rotated according to
|
||||
/// the give rotation.
|
||||
SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
|
||||
SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);
|
||||
|
||||
static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
|
||||
|
||||
/// <summary>
|
||||
/// Convert an angle/axis representation to a swingt
|
||||
/// </summary>
|
||||
/// <param name="angle">The angle</param>
|
||||
/// <param name="axis">The axis</param>
|
||||
/// <returns>The resulting quaternion</returns>
|
||||
static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
|
||||
|
||||
static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
|
||||
|
||||
void Normalize();
|
||||
};
|
||||
|
||||
using SwingTwistSingle = SwingTwistOf<float>;
|
||||
using SwingTwist16 = SwingTwistOf<signed short>;
|
||||
|
||||
#if defined(ARDUINO)
|
||||
using SwingTwist = SwingTwist16;
|
||||
#else
|
||||
using SwingTwist = SwingTwistSingle;
|
||||
#endif
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#endif
|
||||
@ -1,182 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "Vector2.h"
|
||||
#include "Angle.h"
|
||||
#include "FloatSingle.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
// #if defined(AVR)
|
||||
// #include <Arduino.h>
|
||||
// #else
|
||||
#include <math.h>
|
||||
// #endif
|
||||
|
||||
Vector2::Vector2() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
}
|
||||
Vector2::Vector2(float _x, float _y) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
}
|
||||
// Vector2::Vector2(Vec2 v) {
|
||||
// x = v.x;
|
||||
// y = v.y;
|
||||
// }
|
||||
Vector2::Vector2(Vector3 v) {
|
||||
x = v.Right(); // x;
|
||||
y = v.Forward(); // z;
|
||||
}
|
||||
Vector2::Vector2(PolarSingle p) {
|
||||
float horizontalRad = p.angle.InDegrees() * Deg2Rad;
|
||||
float cosHorizontal = cosf(horizontalRad);
|
||||
float sinHorizontal = sinf(horizontalRad);
|
||||
|
||||
x = p.distance * sinHorizontal;
|
||||
y = p.distance * cosHorizontal;
|
||||
}
|
||||
|
||||
Vector2::~Vector2() {}
|
||||
|
||||
const Vector2 Vector2::zero = Vector2(0, 0);
|
||||
const Vector2 Vector2::one = Vector2(1, 1);
|
||||
const Vector2 Vector2::right = Vector2(1, 0);
|
||||
const Vector2 Vector2::left = Vector2(-1, 0);
|
||||
const Vector2 Vector2::up = Vector2(0, 1);
|
||||
const Vector2 Vector2::down = Vector2(0, -1);
|
||||
const Vector2 Vector2::forward = Vector2(0, 1);
|
||||
const Vector2 Vector2::back = Vector2(0, -1);
|
||||
|
||||
bool Vector2::operator==(const Vector2& v) {
|
||||
return (this->x == v.x && this->y == v.y);
|
||||
}
|
||||
|
||||
float Vector2::Magnitude(const Vector2& v) {
|
||||
return sqrtf(v.x * v.x + v.y * v.y);
|
||||
}
|
||||
float Vector2::magnitude() const {
|
||||
return (float)sqrtf(x * x + y * y);
|
||||
}
|
||||
float Vector2::SqrMagnitude(const Vector2& v) {
|
||||
return v.x * v.x + v.y * v.y;
|
||||
}
|
||||
float Vector2::sqrMagnitude() const {
|
||||
return (x * x + y * y);
|
||||
}
|
||||
|
||||
Vector2 Vector2::Normalize(const Vector2& v) {
|
||||
float num = Vector2::Magnitude(v);
|
||||
Vector2 result = Vector2::zero;
|
||||
if (num > Float::epsilon) {
|
||||
result = v / num;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
Vector2 Vector2::normalized() const {
|
||||
float num = this->magnitude();
|
||||
Vector2 result = Vector2::zero;
|
||||
if (num > Float::epsilon) {
|
||||
result = ((Vector2) * this) / num;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
Vector2 Vector2::operator-() {
|
||||
return Vector2(-this->x, -this->y);
|
||||
}
|
||||
|
||||
Vector2 Vector2::operator-(const Vector2& v) const {
|
||||
return Vector2(this->x - v.x, this->y - v.y);
|
||||
}
|
||||
Vector2 Vector2::operator-=(const Vector2& v) {
|
||||
this->x -= v.x;
|
||||
this->y -= v.y;
|
||||
return *this;
|
||||
}
|
||||
Vector2 Vector2::operator+(const Vector2& v) const {
|
||||
return Vector2(this->x + v.x, this->y + v.y);
|
||||
}
|
||||
Vector2 Vector2::operator+=(const Vector2& v) {
|
||||
this->x += v.x;
|
||||
this->y += v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) {
|
||||
return Vector2(v1.x * v2.x, v1.y * v2.y);
|
||||
}
|
||||
// Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
|
||||
// return Vector2(v.x * f, v.y * f);
|
||||
// }
|
||||
// Vector2 Passer::LinearAlgebra::operator*(float f, const Vector2 &v) {
|
||||
// return Vector2(v.x * f, v.y * f);
|
||||
// }
|
||||
Vector2 Vector2::operator*=(float f) {
|
||||
this->x *= f;
|
||||
this->y *= f;
|
||||
return *this;
|
||||
}
|
||||
// Vector2 Passer::LinearAlgebra::operator/(const Vector2 &v, float f) {
|
||||
// return Vector2(v.x / f, v.y / f);
|
||||
// }
|
||||
// Vector2 Passer::LinearAlgebra::operator/(float f, const Vector2 &v) {
|
||||
// return Vector2(v.x / f, v.y / f);
|
||||
// }
|
||||
Vector2 Vector2::operator/=(float f) {
|
||||
this->x /= f;
|
||||
this->y /= f;
|
||||
return *this;
|
||||
}
|
||||
|
||||
float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
|
||||
return v1.x * v2.x + v1.y * v2.y;
|
||||
}
|
||||
|
||||
float Vector2::Distance(const Vector2& v1, const Vector2& v2) {
|
||||
return Magnitude(v1 - v2);
|
||||
}
|
||||
|
||||
float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
|
||||
return (float)fabs(SignedAngle(v1, v2));
|
||||
}
|
||||
float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
|
||||
float sqrMagFrom = v1.sqrMagnitude();
|
||||
float sqrMagTo = v2.sqrMagnitude();
|
||||
|
||||
if (sqrMagFrom == 0 || sqrMagTo == 0)
|
||||
return 0;
|
||||
if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
|
||||
#if defined(AVR)
|
||||
return NAN;
|
||||
#else
|
||||
return nanf("");
|
||||
#endif
|
||||
|
||||
float angleFrom = atan2f(v1.y, v1.x);
|
||||
float angleTo = atan2f(v2.y, v2.x);
|
||||
return -(angleTo - angleFrom) * Rad2Deg;
|
||||
}
|
||||
|
||||
Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
|
||||
float angleRad = a.InDegrees() * Deg2Rad;
|
||||
#if defined(AVR)
|
||||
float sinValue = sin(angleRad);
|
||||
float cosValue = cos(angleRad); // * Angle::Deg2Rad);
|
||||
#else
|
||||
float sinValue = (float)sinf(angleRad);
|
||||
float cosValue = (float)cosf(angleRad);
|
||||
#endif
|
||||
|
||||
float tx = v.x;
|
||||
float ty = v.y;
|
||||
Vector2 r = Vector2((cosValue * tx) - (sinValue * ty),
|
||||
(sinValue * tx) + (cosValue * ty));
|
||||
return r;
|
||||
}
|
||||
|
||||
Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
|
||||
Vector2 v = v1 + (v2 - v1) * f;
|
||||
return v;
|
||||
}
|
||||
@ -1,209 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef VECTOR2_H
|
||||
#define VECTOR2_H
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
extern "C" {
|
||||
/// <summary>
|
||||
/// 2-dimensional Vector representation (C-style)
|
||||
/// </summary>
|
||||
/// This is a C-style implementation
|
||||
/// This uses the right-handed coordinate system.
|
||||
typedef struct Vec2 {
|
||||
/// <summary>
|
||||
/// The right axis of the vector
|
||||
/// </summary>
|
||||
float x;
|
||||
/// <summary>
|
||||
/// The upward/forward axis of the vector
|
||||
/// </summary>
|
||||
float y;
|
||||
|
||||
} Vec2;
|
||||
}
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
struct Vector3;
|
||||
template <typename T>
|
||||
class PolarOf;
|
||||
|
||||
/// @brief A 2-dimensional vector
|
||||
/// @remark This uses the right=handed carthesian coordinate system.
|
||||
/// @note This implementation intentionally avoids the use of x and y
|
||||
struct Vector2 : Vec2 {
|
||||
friend struct Vec2;
|
||||
|
||||
public:
|
||||
/// @brief A new 2-dimensional zero vector
|
||||
Vector2();
|
||||
/// @brief A new 2-dimensional vector
|
||||
/// @param right The distance in the right direction in meters
|
||||
/// @param forward The distance in the forward direction in meters
|
||||
Vector2(float right, float forward);
|
||||
/// @brief Convert a Vector3 to a Vector2
|
||||
/// @param v The 3D vector
|
||||
/// @note This will project the vector to the horizontal plane
|
||||
Vector2(Vector3 v);
|
||||
/// @brief Convert a Polar vector to a 2-dimensional vector
|
||||
/// @param v The vector in polar coordinates
|
||||
Vector2(PolarOf<float> v);
|
||||
|
||||
/// @brief Vector2 destructor
|
||||
~Vector2();
|
||||
|
||||
/// @brief A vector with zero for all axis
|
||||
const static Vector2 zero;
|
||||
/// @brief A vector with one for all axis
|
||||
const static Vector2 one;
|
||||
/// @brief A normalized forward-oriented vector
|
||||
const static Vector2 forward;
|
||||
/// @brief A normalized back-oriented vector
|
||||
const static Vector2 back;
|
||||
/// @brief A normalized right-oriented vector
|
||||
const static Vector2 right;
|
||||
/// @brief A normalized left-oriented vector
|
||||
const static Vector2 left;
|
||||
/// @brief A normalized up-oriented vector
|
||||
/// @note This is a convenience function which is equal to Vector2::forward
|
||||
const static Vector2 up;
|
||||
/// @brief A normalized down-oriented vector
|
||||
/// @note This is a convenience function which is equal to Vector2::down
|
||||
const static Vector2 down;
|
||||
|
||||
/// @brief Check if this vector to the given vector
|
||||
/// @param v The vector to check against
|
||||
/// @return true if it is identical to the given vector
|
||||
/// @note This uses float comparison to check equality which may have strange
|
||||
/// effects. Equality on floats should be avoided.
|
||||
bool operator==(const Vector2& v);
|
||||
|
||||
/// @brief The vector length
|
||||
/// @param v The vector for which you need the length
|
||||
/// @return The vector length
|
||||
static float Magnitude(const Vector2& v);
|
||||
/// @brief The vector length
|
||||
/// @return The vector length
|
||||
float magnitude() const;
|
||||
/// @brief The squared vector length
|
||||
/// @param v The vector for which you need the squared length
|
||||
/// @return The squared vector length
|
||||
/// @remark The squared length is computationally simpler than the real
|
||||
/// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation
|
||||
/// of the squared root of C.
|
||||
static float SqrMagnitude(const Vector2& v);
|
||||
/// @brief The squared vector length
|
||||
/// @return The squared vector length
|
||||
/// @remark The squared length is computationally simpler than the real
|
||||
/// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation
|
||||
/// of the squared root of C.
|
||||
float sqrMagnitude() const;
|
||||
|
||||
/// @brief Convert the vector to a length of 1
|
||||
/// @param v The vector to convert
|
||||
/// @return The vector normalized to a length of 1
|
||||
static Vector2 Normalize(const Vector2& v);
|
||||
/// @brief Convert the vector to a length 1
|
||||
/// @return The vector normalized to a length of 1
|
||||
Vector2 normalized() const;
|
||||
|
||||
/// @brief Negate the vector such that it points in the opposite direction
|
||||
/// @return The negated vector
|
||||
Vector2 operator-();
|
||||
|
||||
/// @brief Subtract a vector from this vector
|
||||
/// @param v The vector to subtract from this vector
|
||||
/// @return The result of the subtraction
|
||||
Vector2 operator-(const Vector2& v) const;
|
||||
Vector2 operator-=(const Vector2& v);
|
||||
/// @brief Add a vector to this vector
|
||||
/// @param v The vector to add to this vector
|
||||
/// @return The result of the addition
|
||||
Vector2 operator+(const Vector2& v) const;
|
||||
Vector2 operator+=(const Vector2& v);
|
||||
|
||||
/// @brief Scale the vector using another vector
|
||||
/// @param v1 The vector to scale
|
||||
/// @param v2 A vector with the scaling factors
|
||||
/// @return The scaled vector
|
||||
/// @remark Each component of the vector v1 will be multiplied with the
|
||||
/// matching component from the scaling vector v2.
|
||||
static Vector2 Scale(const Vector2& v1, const Vector2& v2);
|
||||
/// @brief Scale the vector uniformly up
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled vector
|
||||
/// @remark Each component of the vector will be multipled with the same
|
||||
/// factor f.
|
||||
friend Vector2 operator*(const Vector2& v, float f) {
|
||||
return Vector2(v.x * f, v.y * f);
|
||||
}
|
||||
friend Vector2 operator*(float f, const Vector2& v) {
|
||||
return Vector2(v.x * f, v.y * f);
|
||||
// return Vector2(f * v.x, f * v.y);
|
||||
}
|
||||
Vector2 operator*=(float f);
|
||||
/// @brief Scale the vector uniformly down
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled vector
|
||||
/// @remark Each componet of the vector will be divided by the same factor.
|
||||
friend Vector2 operator/(const Vector2& v, float f) {
|
||||
return Vector2(v.x / f, v.y / f);
|
||||
}
|
||||
friend Vector2 operator/(float f, const Vector2& v) {
|
||||
return Vector2(f / v.x, f / v.y);
|
||||
}
|
||||
Vector2 operator/=(float f);
|
||||
|
||||
/// @brief The dot product of two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The dot product of the two vectors
|
||||
static float Dot(const Vector2& v1, const Vector2& v2);
|
||||
|
||||
/// @brief The distance between two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The distance between the two vectors
|
||||
static float Distance(const Vector2& v1, const Vector2& v2);
|
||||
|
||||
/// @brief The angle between two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The angle between the two vectors
|
||||
/// @remark This reterns an unsigned angle which is the shortest distance
|
||||
/// between the two vectors. Use Vector2::SignedAngle if a signed angle is
|
||||
/// needed.
|
||||
static float Angle(const Vector2& v1, const Vector2& v2);
|
||||
/// @brief The signed angle between two vectors
|
||||
/// @param v1 The starting vector
|
||||
/// @param v2 The ending vector
|
||||
/// @return The signed angle between the two vectors
|
||||
static float SignedAngle(const Vector2& v1, const Vector2& v2);
|
||||
|
||||
/// @brief Rotate the vector
|
||||
/// @param v The vector to rotate
|
||||
/// @param a The angle in degrees to rotate
|
||||
/// @return The rotated vector
|
||||
static Vector2 Rotate(const Vector2& v, AngleSingle a);
|
||||
|
||||
/// @brief Lerp (linear interpolation) between two vectors
|
||||
/// @param v1 The starting vector
|
||||
/// @param v2 The end vector
|
||||
/// @param f The interpolation distance
|
||||
/// @return The lerped vector
|
||||
/// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
|
||||
/// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
|
||||
/// between *v1* and *v2* etc.
|
||||
static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
|
||||
};
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#include "Polar.h"
|
||||
|
||||
#endif
|
||||
@ -1,224 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#include "Vector3.h"
|
||||
#include "Angle.h"
|
||||
#include "Spherical.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
const float Deg2Rad = 0.0174532924F;
|
||||
const float Rad2Deg = 57.29578F;
|
||||
const float epsilon = 1E-05f;
|
||||
|
||||
Vector3::Vector3() {
|
||||
this->x = 0;
|
||||
this->y = 0;
|
||||
this->z = 0;
|
||||
}
|
||||
|
||||
Vector3::Vector3(float right, float up, float forward) {
|
||||
this->x = right;
|
||||
this->y = up;
|
||||
this->z = forward;
|
||||
}
|
||||
|
||||
Vector3::Vector3(Vector2 v) {
|
||||
this->x = v.x;
|
||||
this->y = 0.0f;
|
||||
this->z = v.y;
|
||||
}
|
||||
|
||||
Vector3::Vector3(SphericalOf<float> s) {
|
||||
float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad;
|
||||
float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
|
||||
float cosVertical = cosf(verticalRad);
|
||||
float sinVertical = sinf(verticalRad);
|
||||
float cosHorizontal = cosf(horizontalRad);
|
||||
float sinHorizontal = sinf(horizontalRad);
|
||||
|
||||
x = s.distance * sinVertical * sinHorizontal;
|
||||
y = s.distance * cosVertical;
|
||||
z = s.distance * sinVertical * cosHorizontal;
|
||||
// Vector3 v = Vector3(s.distance * sinVertical * sinHorizontal,
|
||||
// s.distance * cosVertical,
|
||||
// );
|
||||
// return v;
|
||||
}
|
||||
|
||||
Vector3::~Vector3() {}
|
||||
|
||||
const Vector3 Vector3::zero = Vector3(0, 0, 0);
|
||||
const Vector3 Vector3::one = Vector3(1, 1, 1);
|
||||
const Vector3 Vector3::right = Vector3(1, 0, 0);
|
||||
const Vector3 Vector3::left = Vector3(-1, 0, 0);
|
||||
const Vector3 Vector3::up = Vector3(0, 1, 0);
|
||||
const Vector3 Vector3::down = Vector3(0, -1, 0);
|
||||
const Vector3 Vector3::forward = Vector3(0, 0, 1);
|
||||
const Vector3 Vector3::back = Vector3(0, 0, -1);
|
||||
|
||||
// inline float Vector3::Forward() { return z; }
|
||||
// inline float Vector3::Up() { return y; }
|
||||
// inline float Vector3::Right() { return x; }
|
||||
// Vector3 Vector3::FromHorizontal(const Vector2 &v) {
|
||||
// return Vector3(v.x, 0, v.y);
|
||||
// }
|
||||
|
||||
float Vector3::Magnitude(const Vector3& v) {
|
||||
return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
|
||||
}
|
||||
float Vector3::magnitude() const {
|
||||
return (float)sqrtf(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
float Vector3::SqrMagnitude(const Vector3& v) {
|
||||
return v.x * v.x + v.y * v.y + v.z * v.z;
|
||||
}
|
||||
float Vector3::sqrMagnitude() const {
|
||||
return (x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
Vector3 Vector3::Normalize(const Vector3& v) {
|
||||
float num = Vector3::Magnitude(v);
|
||||
Vector3 result = Vector3::zero;
|
||||
if (num > epsilon) {
|
||||
result = v / num;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
Vector3 Vector3::normalized() const {
|
||||
float num = this->magnitude();
|
||||
Vector3 result = Vector3::zero;
|
||||
if (num > epsilon) {
|
||||
result = ((Vector3) * this) / num;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator-() const {
|
||||
return Vector3(-this->x, -this->y, -this->z);
|
||||
}
|
||||
|
||||
Vector3 Vector3::operator-(const Vector3& v) const {
|
||||
return Vector3(this->x - v.x, this->y - v.y, this->z - v.z);
|
||||
}
|
||||
Vector3 Vector3::operator-=(const Vector3& v) {
|
||||
this->x -= v.x;
|
||||
this->y -= v.y;
|
||||
this->z -= v.z;
|
||||
return *this;
|
||||
}
|
||||
Vector3 Vector3::operator+(const Vector3& v) const {
|
||||
return Vector3(this->x + v.x, this->y + v.y, this->z + v.z);
|
||||
}
|
||||
Vector3 Vector3::operator+=(const Vector3& v) {
|
||||
this->x += v.x;
|
||||
this->y += v.y;
|
||||
this->z += v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector3 Vector3::Scale(const Vector3& v1, const Vector3& v2) {
|
||||
return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
|
||||
}
|
||||
// Vector3 Passer::LinearAlgebra::operator*(const Vector3 &v, float f) {
|
||||
// return Vector3(v.x * f, v.y * f, v.z * f);
|
||||
// }
|
||||
// Vector3 Passer::LinearAlgebra::operator*(float f, const Vector3 &v) {
|
||||
// return Vector3(v.x * f, v.y * f, v.z * f);
|
||||
// }
|
||||
Vector3 Vector3::operator*=(float f) {
|
||||
this->x *= f;
|
||||
this->y *= f;
|
||||
this->z *= f;
|
||||
return *this;
|
||||
}
|
||||
// Vector3 Passer::LinearAlgebra::operator/(const Vector3 &v, float f) {
|
||||
// return Vector3(v.x / f, v.y / f, v.z / f);
|
||||
// }
|
||||
// Vector3 Passer::LinearAlgebra::operator/(float f, const Vector3 &v) {
|
||||
// return Vector3(v.x / f, v.y / f, v.z / f);
|
||||
// }
|
||||
Vector3 Vector3::operator/=(float f) {
|
||||
this->x /= f;
|
||||
this->y /= f;
|
||||
this->z /= f;
|
||||
return *this;
|
||||
}
|
||||
|
||||
float Vector3::Dot(const Vector3& v1, const Vector3& v2) {
|
||||
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
|
||||
}
|
||||
|
||||
bool Vector3::operator==(const Vector3& v) const {
|
||||
return (this->x == v.x && this->y == v.y && this->z == v.z);
|
||||
}
|
||||
|
||||
float Vector3::Distance(const Vector3& v1, const Vector3& v2) {
|
||||
return Magnitude(v1 - v2);
|
||||
}
|
||||
|
||||
Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) {
|
||||
return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z,
|
||||
v1.x * v2.y - v1.y * v2.x);
|
||||
}
|
||||
|
||||
Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
|
||||
float sqrMagnitude = Dot(n, n);
|
||||
if (sqrMagnitude < epsilon)
|
||||
return Vector3::zero;
|
||||
else {
|
||||
float dot = Dot(v, n);
|
||||
Vector3 r = n * dot / sqrMagnitude;
|
||||
return r;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 Vector3::ProjectOnPlane(const Vector3& v, const Vector3& n) {
|
||||
Vector3 r = v - Project(v, n);
|
||||
return r;
|
||||
}
|
||||
|
||||
float clamp(float x, float lower, float upper) {
|
||||
float lowerClamp = fmaxf(x, lower);
|
||||
float upperClamp = fminf(upper, lowerClamp);
|
||||
return upperClamp;
|
||||
}
|
||||
|
||||
AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
|
||||
float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
|
||||
if (denominator < epsilon)
|
||||
return AngleOf<float>();
|
||||
|
||||
float dot = Vector3::Dot(v1, v2);
|
||||
float fraction = dot / denominator;
|
||||
if (isnan(fraction))
|
||||
return AngleOf<float>::Degrees(
|
||||
fraction); // short cut to returning NaN universally
|
||||
|
||||
float cdot = clamp(fraction, -1.0, 1.0);
|
||||
float r = ((float)acos(cdot));
|
||||
return AngleOf<float>::Radians(r);
|
||||
}
|
||||
|
||||
AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
|
||||
const Vector3& v2,
|
||||
const Vector3& axis) {
|
||||
// angle in [0,180]
|
||||
AngleOf<float> angle = Vector3::Angle(v1, v2);
|
||||
|
||||
Vector3 cross = Vector3::Cross(v1, v2);
|
||||
float b = Vector3::Dot(axis, cross);
|
||||
float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F);
|
||||
|
||||
// angle in [-179,180]
|
||||
AngleOf<float> signed_angle = angle * signd;
|
||||
|
||||
return AngleOf<float>(signed_angle);
|
||||
}
|
||||
|
||||
Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) {
|
||||
Vector3 v = v1 + (v2 - v1) * f;
|
||||
return v;
|
||||
}
|
||||
@ -1,233 +0,0 @@
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef VECTOR3_H
|
||||
#define VECTOR3_H
|
||||
|
||||
#include "Vector2.h"
|
||||
|
||||
extern "C" {
|
||||
/// <summary>
|
||||
/// 3-dimensional Vector representation (C-style)
|
||||
/// </summary>
|
||||
/// This is a C-style implementation
|
||||
/// This uses the right-handed coordinate system.
|
||||
typedef struct Vec3 {
|
||||
protected:
|
||||
/// <summary>
|
||||
/// The right axis of the vector
|
||||
/// </summary>
|
||||
float x;
|
||||
/// <summary>
|
||||
/// The upward axis of the vector
|
||||
/// </summary>
|
||||
float y;
|
||||
/// <summary>
|
||||
/// The forward axis of the vector
|
||||
/// </summary>
|
||||
float z;
|
||||
|
||||
} Vec3;
|
||||
}
|
||||
|
||||
namespace LinearAlgebra {
|
||||
|
||||
template <typename T>
|
||||
class SphericalOf;
|
||||
|
||||
/// @brief A 3-dimensional vector
|
||||
/// @remark This uses a right-handed carthesian coordinate system.
|
||||
/// @note This implementation intentionally avoids the use of x, y and z values.
|
||||
struct Vector3 : Vec3 {
|
||||
friend struct Vec3;
|
||||
|
||||
public:
|
||||
/// @brief A new 3-dimensional zero vector
|
||||
Vector3();
|
||||
/// @brief A new 3-dimensional vector
|
||||
/// @param right The distance in the right direction in meters
|
||||
/// @param up The distance in the upward direction in meters
|
||||
/// @param forward The distance in the forward direction in meters
|
||||
Vector3(float right, float up, float forward);
|
||||
/// @brief Convert a 2-dimenstional vector to a 3-dimensional vector
|
||||
/// @param v The vector to convert
|
||||
Vector3(Vector2 v);
|
||||
/// @brief Convert vector in spherical coordinates to 3d carthesian
|
||||
/// coordinates
|
||||
/// @param v The vector to convert
|
||||
Vector3(SphericalOf<float> v);
|
||||
|
||||
/// @brief Vector3 destructor
|
||||
~Vector3();
|
||||
|
||||
/// @brief A vector with zero for all axis
|
||||
const static Vector3 zero;
|
||||
/// @brief A vector with one for all axis
|
||||
const static Vector3 one;
|
||||
/// @brief A normalized forward-oriented vector
|
||||
const static Vector3 forward;
|
||||
/// @brief A normalized back-oriented vector
|
||||
const static Vector3 back;
|
||||
/// @brief A normalized right-oriented vector
|
||||
const static Vector3 right;
|
||||
/// @brief A normalized left-oriented vector
|
||||
const static Vector3 left;
|
||||
/// @brief A normalized up-oriented vector
|
||||
const static Vector3 up;
|
||||
/// @brief A normalized down-oriented vector
|
||||
const static Vector3 down;
|
||||
|
||||
// Access functions which are intended to replace the use of XYZ
|
||||
inline float Forward() const { return z; };
|
||||
inline float Up() const { return y; };
|
||||
inline float Right() const { return x; };
|
||||
|
||||
/// @brief Check if this vector to the given vector
|
||||
/// @param v The vector to check against
|
||||
/// @return true if it is identical to the given vector
|
||||
/// @note This uses float comparison to check equality which may have strange
|
||||
/// effects. Equality on floats should be avoided.
|
||||
bool operator==(const Vector3& v) const;
|
||||
|
||||
/// @brief The vector length
|
||||
/// @param v The vector for which you need the length
|
||||
/// @return The vector length
|
||||
static float Magnitude(const Vector3& v);
|
||||
/// @brief The vector length
|
||||
/// @return The vector length
|
||||
float magnitude() const;
|
||||
/// @brief The squared vector length
|
||||
/// @param v The vector for which you need the length
|
||||
/// @return The squared vector length
|
||||
/// @remark The squared length is computationally simpler than the real
|
||||
/// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
|
||||
/// calculation of the squared root of C.
|
||||
static float SqrMagnitude(const Vector3& v);
|
||||
/// @brief The squared vector length
|
||||
/// @return The squared vector length
|
||||
/// @remark The squared length is computationally simpler than the real
|
||||
/// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
|
||||
/// calculation of the squared root of C.
|
||||
float sqrMagnitude() const;
|
||||
|
||||
/// @brief Convert the vector to a length of 1
|
||||
/// @param v The vector to convert
|
||||
/// @return The vector normalized to a length of 1
|
||||
static Vector3 Normalize(const Vector3& v);
|
||||
/// @brief Convert the vector to a length of 1
|
||||
/// @return The vector normalized to a length of 1
|
||||
Vector3 normalized() const;
|
||||
|
||||
/// @brief Negate te vector such that it points in the opposite direction
|
||||
/// @return The negated vector
|
||||
Vector3 operator-() const;
|
||||
|
||||
/// @brief Subtract a vector from this vector
|
||||
/// @param v The vector to subtract from this vector
|
||||
/// @return The result of this subtraction
|
||||
Vector3 operator-(const Vector3& v) const;
|
||||
Vector3 operator-=(const Vector3& v);
|
||||
/// @brief Add a vector to this vector
|
||||
/// @param v The vector to add to this vector
|
||||
/// @return The result of the addition
|
||||
Vector3 operator+(const Vector3& v) const;
|
||||
Vector3 operator+=(const Vector3& v);
|
||||
|
||||
/// @brief Scale the vector using another vector
|
||||
/// @param v1 The vector to scale
|
||||
/// @param v2 A vector with the scaling factors
|
||||
/// @return The scaled vector
|
||||
/// @remark Each component of the vector v1 will be multiplied with the
|
||||
/// matching component from the scaling vector v2.
|
||||
static Vector3 Scale(const Vector3& v1, const Vector3& v2);
|
||||
/// @brief Scale the vector uniformly up
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled vector
|
||||
/// @remark Each component of the vector will be multipled with the same
|
||||
/// factor f.
|
||||
friend Vector3 operator*(const Vector3& v, float f) {
|
||||
return Vector3(v.x * f, v.y * f, v.z * f);
|
||||
}
|
||||
friend Vector3 operator*(float f, const Vector3& v) {
|
||||
// return Vector3(f * v.x, f * v.y, f * v.z);
|
||||
return Vector3(v.x * f, v.y * f, v.z * f);
|
||||
}
|
||||
Vector3 operator*=(float f);
|
||||
/// @brief Scale the vector uniformly down
|
||||
/// @param f The scaling factor
|
||||
/// @return The scaled vector
|
||||
/// @remark Each componet of the vector will be divided by the same factor.
|
||||
friend Vector3 operator/(const Vector3& v, float f) {
|
||||
return Vector3(v.x / f, v.y / f, v.z / f);
|
||||
}
|
||||
friend Vector3 operator/(float f, const Vector3& v) {
|
||||
// return Vector3(f / v.x, f / v.y, f / v.z);
|
||||
return Vector3(v.x / f, v.y / f, v.z / f);
|
||||
}
|
||||
Vector3 operator/=(float f);
|
||||
|
||||
/// @brief The distance between two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The distance between the two vectors
|
||||
static float Distance(const Vector3& v1, const Vector3& v2);
|
||||
|
||||
/// @brief The dot product of two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The dot product of the two vectors
|
||||
static float Dot(const Vector3& v1, const Vector3& v2);
|
||||
|
||||
/// @brief The cross product of two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The cross product of the two vectors
|
||||
static Vector3 Cross(const Vector3& v1, const Vector3& v2);
|
||||
|
||||
/// @brief Project the vector on another vector
|
||||
/// @param v The vector to project
|
||||
/// @param n The normal vecto to project on
|
||||
/// @return The projected vector
|
||||
static Vector3 Project(const Vector3& v, const Vector3& n);
|
||||
/// @brief Project the vector on a plane defined by a normal orthogonal to the
|
||||
/// plane.
|
||||
/// @param v The vector to project
|
||||
/// @param n The normal of the plane to project on
|
||||
/// @return Teh projected vector
|
||||
static Vector3 ProjectOnPlane(const Vector3& v, const Vector3& n);
|
||||
|
||||
/// @brief The angle between two vectors
|
||||
/// @param v1 The first vector
|
||||
/// @param v2 The second vector
|
||||
/// @return The angle between the two vectors
|
||||
/// @remark This reterns an unsigned angle which is the shortest distance
|
||||
/// between the two vectors. Use Vector3::SignedAngle if a signed angle is
|
||||
/// needed.
|
||||
static AngleOf<float> Angle(const Vector3& v1, const Vector3& v2);
|
||||
/// @brief The signed angle between two vectors
|
||||
/// @param v1 The starting vector
|
||||
/// @param v2 The ending vector
|
||||
/// @param axis The axis to rotate around
|
||||
/// @return The signed angle between the two vectors
|
||||
static AngleOf<float> SignedAngle(const Vector3& v1,
|
||||
const Vector3& v2,
|
||||
const Vector3& axis);
|
||||
|
||||
/// @brief Lerp (linear interpolation) between two vectors
|
||||
/// @param v1 The starting vector
|
||||
/// @param v2 The ending vector
|
||||
/// @param f The interpolation distance
|
||||
/// @return The lerped vector
|
||||
/// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
|
||||
/// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
|
||||
/// between *v1* and *v2* etc.
|
||||
static Vector3 Lerp(const Vector3& v1, const Vector3& v2, float f);
|
||||
};
|
||||
|
||||
} // namespace LinearAlgebra
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#include "Spherical.h"
|
||||
|
||||
#endif
|
||||
@ -1 +0,0 @@
|
||||
COMPONENT_ADD_INCLUDEDIRS = .
|
||||
@ -1,250 +0,0 @@
|
||||
//
|
||||
// FILE: float16.cpp
|
||||
// AUTHOR: Rob Tillaart
|
||||
// VERSION: 0.1.8
|
||||
// PURPOSE: library for Float16s for Arduino
|
||||
// URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format
|
||||
|
||||
#include "float16.h"
|
||||
// #include <limits>
|
||||
#include <math.h>
|
||||
|
||||
// CONSTRUCTOR
|
||||
float16::float16(float f) { _value = f32tof16(f); }
|
||||
|
||||
// PRINTING
|
||||
// size_t float16::printTo(Print& p) const
|
||||
// {
|
||||
// double d = this->f16tof32(_value);
|
||||
// return p.print(d, _decimals);
|
||||
// }
|
||||
|
||||
float float16::toFloat() const { return f16tof32(_value); }
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
//
|
||||
// EQUALITIES
|
||||
//
|
||||
bool float16::operator==(const float16 &f) { return (_value == f._value); }
|
||||
|
||||
bool float16::operator!=(const float16 &f) { return (_value != f._value); }
|
||||
|
||||
bool float16::operator>(const float16 &f) {
|
||||
if ((_value & 0x8000) && (f._value & 0x8000))
|
||||
return _value < f._value;
|
||||
if (_value & 0x8000)
|
||||
return false;
|
||||
if (f._value & 0x8000)
|
||||
return true;
|
||||
return _value > f._value;
|
||||
}
|
||||
|
||||
bool float16::operator>=(const float16 &f) {
|
||||
if ((_value & 0x8000) && (f._value & 0x8000))
|
||||
return _value <= f._value;
|
||||
if (_value & 0x8000)
|
||||
return false;
|
||||
if (f._value & 0x8000)
|
||||
return true;
|
||||
return _value >= f._value;
|
||||
}
|
||||
|
||||
bool float16::operator<(const float16 &f) {
|
||||
if ((_value & 0x8000) && (f._value & 0x8000))
|
||||
return _value > f._value;
|
||||
if (_value & 0x8000)
|
||||
return true;
|
||||
if (f._value & 0x8000)
|
||||
return false;
|
||||
return _value < f._value;
|
||||
}
|
||||
|
||||
bool float16::operator<=(const float16 &f) {
|
||||
if ((_value & (uint16_t)0x8000) && (f._value & (uint16_t)0x8000))
|
||||
return _value >= f._value;
|
||||
if (_value & 0x8000)
|
||||
return true;
|
||||
if (f._value & 0x8000)
|
||||
return false;
|
||||
return _value <= f._value;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
//
|
||||
// NEGATION
|
||||
//
|
||||
float16 float16::operator-() {
|
||||
float16 f16;
|
||||
f16.setBinary(_value ^ 0x8000);
|
||||
return f16;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
//
|
||||
// MATH
|
||||
//
|
||||
float16 float16::operator+(const float16 &f) {
|
||||
return float16(this->toFloat() + f.toFloat());
|
||||
}
|
||||
|
||||
float16 float16::operator-(const float16 &f) {
|
||||
return float16(this->toFloat() - f.toFloat());
|
||||
}
|
||||
|
||||
float16 float16::operator*(const float16 &f) {
|
||||
return float16(this->toFloat() * f.toFloat());
|
||||
}
|
||||
|
||||
float16 float16::operator/(const float16 &f) {
|
||||
return float16(this->toFloat() / f.toFloat());
|
||||
}
|
||||
|
||||
float16 &float16::operator+=(const float16 &f) {
|
||||
*this = this->toFloat() + f.toFloat();
|
||||
return *this;
|
||||
}
|
||||
|
||||
float16 &float16::operator-=(const float16 &f) {
|
||||
*this = this->toFloat() - f.toFloat();
|
||||
return *this;
|
||||
}
|
||||
|
||||
float16 &float16::operator*=(const float16 &f) {
|
||||
*this = this->toFloat() * f.toFloat();
|
||||
return *this;
|
||||
}
|
||||
|
||||
float16 &float16::operator/=(const float16 &f) {
|
||||
*this = this->toFloat() / f.toFloat();
|
||||
return *this;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
//
|
||||
// MATH HELPER FUNCTIONS
|
||||
//
|
||||
int float16::sign() {
|
||||
if (_value & 0x8000)
|
||||
return -1;
|
||||
if (_value & 0xFFFF)
|
||||
return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); }
|
||||
|
||||
bool float16::isNaN() {
|
||||
if ((_value & 0x7C00) != 0x7C00)
|
||||
return false;
|
||||
if ((_value & 0x03FF) == 0x0000)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); }
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
//
|
||||
// CORE CONVERSION
|
||||
//
|
||||
float float16::f16tof32(uint16_t _value) const {
|
||||
uint16_t sgn, man;
|
||||
int exp;
|
||||
float f;
|
||||
|
||||
sgn = (_value & 0x8000) > 0;
|
||||
exp = (_value & 0x7C00) >> 10;
|
||||
man = (_value & 0x03FF);
|
||||
|
||||
// ZERO
|
||||
if ((_value & 0x7FFF) == 0) {
|
||||
return sgn ? -0.0f : 0.0f;
|
||||
}
|
||||
// NAN & INF
|
||||
if (exp == 0x001F) {
|
||||
if (man == 0)
|
||||
return sgn ? -INFINITY : INFINITY;
|
||||
else
|
||||
return NAN;
|
||||
}
|
||||
|
||||
// SUBNORMAL/NORMAL
|
||||
if (exp == 0)
|
||||
f = 0;
|
||||
else
|
||||
f = 1;
|
||||
|
||||
// PROCESS MANTISSE
|
||||
for (int i = 9; i >= 0; i--) {
|
||||
f *= 2;
|
||||
if (man & (1 << i))
|
||||
f = f + 1;
|
||||
}
|
||||
f = f * powf(2.0f, (float)(exp - 25));
|
||||
if (exp == 0) {
|
||||
f = f * powf(2.0f, -13); // 5.96046447754e-8;
|
||||
}
|
||||
return sgn ? -f : f;
|
||||
}
|
||||
|
||||
uint16_t float16::f32tof16(float f) const {
|
||||
// untested code, but will avoid strict aliasing warning
|
||||
// union {
|
||||
// float f;
|
||||
// uint32_t t;
|
||||
// } u;
|
||||
// u.f = f;
|
||||
// uint32_t t = u.t;
|
||||
uint32_t t = *(uint32_t *)&f;
|
||||
// man bits = 10; but we keep 11 for rounding
|
||||
uint16_t man = (t & 0x007FFFFF) >> 12;
|
||||
int16_t exp = (t & 0x7F800000) >> 23;
|
||||
bool sgn = (t & 0x80000000);
|
||||
|
||||
// handle 0
|
||||
if ((t & 0x7FFFFFFF) == 0) {
|
||||
return sgn ? 0x8000 : 0x0000;
|
||||
}
|
||||
// denormalized float32 does not fit in float16
|
||||
if (exp == 0x00) {
|
||||
return sgn ? 0x8000 : 0x0000;
|
||||
}
|
||||
// handle infinity & NAN
|
||||
if (exp == 0x00FF) {
|
||||
if (man)
|
||||
return 0xFE00; // NAN
|
||||
return sgn ? 0xFC00 : 0x7C00; // -INF : INF
|
||||
}
|
||||
|
||||
// normal numbers
|
||||
exp = exp - 127 + 15;
|
||||
// overflow does not fit => INF
|
||||
if (exp > 30) {
|
||||
return sgn ? 0xFC00 : 0x7C00; // -INF : INF
|
||||
}
|
||||
// subnormal numbers
|
||||
if (exp < -38) {
|
||||
return sgn ? 0x8000 : 0x0000; // -0 or 0 ? just 0 ?
|
||||
}
|
||||
if (exp <= 0) // subnormal
|
||||
{
|
||||
man >>= (exp + 14);
|
||||
// rounding
|
||||
man++;
|
||||
man >>= 1;
|
||||
if (sgn)
|
||||
return 0x8000 | man;
|
||||
return man;
|
||||
}
|
||||
|
||||
// normal
|
||||
// TODO rounding
|
||||
exp <<= 10;
|
||||
man++;
|
||||
man >>= 1;
|
||||
if (sgn)
|
||||
return 0x8000 | exp | man;
|
||||
return exp | man;
|
||||
}
|
||||
|
||||
// -- END OF FILE --
|
||||
@ -1,74 +0,0 @@
|
||||
#pragma once
|
||||
//
|
||||
// FILE: float16.h
|
||||
// AUTHOR: Rob Tillaart
|
||||
// VERSION: 0.1.8
|
||||
// PURPOSE: Arduino library to implement float16 data type.
|
||||
// half-precision floating point format,
|
||||
// used for efficient storage and transport.
|
||||
// URL: https://github.com/RobTillaart/float16
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define FLOAT16_LIB_VERSION (F("0.1.8"))
|
||||
|
||||
// typedef uint16_t _fp16;
|
||||
|
||||
class float16 {
|
||||
public:
|
||||
// Constructors
|
||||
float16(void) { _value = 0x0000; };
|
||||
float16(float f);
|
||||
float16(const float16 &f) { _value = f._value; };
|
||||
|
||||
// Conversion
|
||||
float toFloat(void) const;
|
||||
// access the 2 byte representation.
|
||||
uint16_t getBinary() { return _value; };
|
||||
void setBinary(uint16_t u) { _value = u; };
|
||||
|
||||
// Printable
|
||||
// size_t printTo(Print &p) const;
|
||||
void setDecimals(uint8_t d) { _decimals = d; };
|
||||
uint8_t getDecimals() { return _decimals; };
|
||||
|
||||
// equalities
|
||||
bool operator==(const float16 &f);
|
||||
bool operator!=(const float16 &f);
|
||||
|
||||
bool operator>(const float16 &f);
|
||||
bool operator>=(const float16 &f);
|
||||
bool operator<(const float16 &f);
|
||||
bool operator<=(const float16 &f);
|
||||
|
||||
// negation
|
||||
float16 operator-();
|
||||
|
||||
// basic math
|
||||
float16 operator+(const float16 &f);
|
||||
float16 operator-(const float16 &f);
|
||||
float16 operator*(const float16 &f);
|
||||
float16 operator/(const float16 &f);
|
||||
|
||||
float16 &operator+=(const float16 &f);
|
||||
float16 &operator-=(const float16 &f);
|
||||
float16 &operator*=(const float16 &f);
|
||||
float16 &operator/=(const float16 &f);
|
||||
|
||||
// math helper functions
|
||||
int sign(); // 1 = positive 0 = zero -1 = negative.
|
||||
bool isZero();
|
||||
bool isNaN();
|
||||
bool isInf();
|
||||
|
||||
// CORE CONVERSION
|
||||
// should be private but for testing...
|
||||
float f16tof32(uint16_t) const;
|
||||
uint16_t f32tof16(float) const;
|
||||
|
||||
private:
|
||||
uint8_t _decimals = 4;
|
||||
uint16_t _value;
|
||||
};
|
||||
|
||||
// -- END OF FILE --
|
||||
@ -1,241 +0,0 @@
|
||||
#if GTEST
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Angle16, Construct) {
|
||||
float angle = 0.0F;
|
||||
Angle16 a = Angle16::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = -180.0F;
|
||||
a = Angle16::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 270.0F;
|
||||
a = Angle16::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
|
||||
}
|
||||
|
||||
TEST(Angle16, Negate) {
|
||||
float angle = 0;
|
||||
Angle16 a = Angle16::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 90.0F;
|
||||
a = Angle16::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
|
||||
}
|
||||
|
||||
TEST(Angle16, Subtract) {
|
||||
Angle16 a = Angle16::Degrees(0);
|
||||
Angle16 b = Angle16::Degrees(45.0F);
|
||||
Angle16 r = a - b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
|
||||
}
|
||||
|
||||
TEST(Angle16, Add) {
|
||||
Angle16 a = Angle16::Degrees(-45);
|
||||
Angle16 b = Angle16::Degrees(45.0F);
|
||||
Angle16 r = a + b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
|
||||
}
|
||||
|
||||
TEST(Angle16, Compare) {
|
||||
Angle16 a = Angle16::Degrees(45);
|
||||
bool r = false;
|
||||
|
||||
r = a > Angle16::Degrees(0);
|
||||
EXPECT_TRUE(r) << "45 > 0";
|
||||
|
||||
r = a > Angle16::Degrees(90);
|
||||
EXPECT_FALSE(r) << "45 > 90";
|
||||
|
||||
r = a > Angle16::Degrees(-90);
|
||||
EXPECT_TRUE(r) << "45 > -90";
|
||||
}
|
||||
|
||||
TEST(Angle16, Normalize) {
|
||||
Angle16 r = Angle16();
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(90.0f));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(-90));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(270));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(270 + 360));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(-270));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(-270 - 360));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
|
||||
|
||||
if (false) { // std::numeric_limits<float>::is_iec559) {
|
||||
// Infinites are not supported
|
||||
r = Angle16::Normalize(Angle16::Degrees(FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
|
||||
|
||||
r = Angle16::Normalize(Angle16::Degrees(-FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Angle16, Clamp) {
|
||||
Angle16 r = Angle16();
|
||||
|
||||
// Clamp(1, 0, 2) will fail because Angle16 does not have enough resolution
|
||||
// for this. Instead we use Clamp(10, 0, 20) etc.
|
||||
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
|
||||
Angle16::Degrees(20));
|
||||
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 10 0 20";
|
||||
|
||||
r = Angle16::Clamp(Angle16::Degrees(-10), Angle16::Degrees(0),
|
||||
Angle16::Degrees(20));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20";
|
||||
|
||||
r = Angle16::Clamp(Angle16::Degrees(30), Angle16::Degrees(0),
|
||||
Angle16::Degrees(20));
|
||||
EXPECT_NEAR(r.InDegrees(), 20, 1.0e-2) << "Clamp 30 0 20";
|
||||
|
||||
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
|
||||
Angle16::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0";
|
||||
|
||||
r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(0),
|
||||
Angle16::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
|
||||
|
||||
r = Angle16::Clamp(Angle16::Degrees(0), Angle16::Degrees(10),
|
||||
Angle16::Degrees(-10));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
|
||||
|
||||
if (false) { // std::numeric_limits<float>::is_iec559) {
|
||||
// Infinites are not supported
|
||||
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
|
||||
Angle16::Degrees(FLOAT_INFINITY));
|
||||
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 0 INFINITY";
|
||||
|
||||
r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(-FLOAT_INFINITY),
|
||||
Angle16::Degrees(10));
|
||||
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-2) << "Clamp 1 -INFINITY 1";
|
||||
}
|
||||
}
|
||||
|
||||
// TEST(Angle16, Difference) {
|
||||
// Angle16 r = 0;
|
||||
|
||||
// r = Angle16::Difference(0, 90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
|
||||
|
||||
// r = Angle16::Difference(0, -90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
|
||||
|
||||
// r = Angle16::Difference(0, 270);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
|
||||
|
||||
// r = Angle16::Difference(0, -270);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
|
||||
|
||||
// r = Angle16::Difference(90, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
|
||||
|
||||
// r = Angle16::Difference(-90, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
|
||||
|
||||
// r = Angle16::Difference(0, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
|
||||
|
||||
// r = Angle16::Difference(90, 90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
|
||||
|
||||
// if (std::numeric_limits<float>::is_iec559) {
|
||||
// r = Angle16::Difference(0, INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
|
||||
|
||||
// r = Angle16::Difference(0, -INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
|
||||
|
||||
// r = Angle16::Difference(-INFINITY, INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
|
||||
// INFINITY";
|
||||
// }
|
||||
// }
|
||||
|
||||
TEST(Angle16, MoveTowards) {
|
||||
Angle16 r = Angle16();
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 30);
|
||||
EXPECT_NEAR(r.InDegrees(), 30, 1.0e-2) << "MoveTowards 0 90 30";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 90);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), 180);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 270);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -90);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -180);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90), -270);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90), 0);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 0);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
|
||||
|
||||
if (false) { // std::numeric_limits<float>::is_iec559) {
|
||||
// infinites are not supported
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90),
|
||||
FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0),
|
||||
Angle16::Degrees(FLOAT_INFINITY), 30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(-90),
|
||||
-FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
|
||||
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
|
||||
|
||||
r = Angle16::MoveTowards(Angle16::Degrees(0),
|
||||
Angle16::Degrees(-FLOAT_INFINITY), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@ -1,241 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Angle8, Construct) {
|
||||
float angle = 0.0F;
|
||||
Angle8 a = Angle8::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = -180.0F;
|
||||
a = Angle8::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 270.0F;
|
||||
a = Angle8::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
|
||||
}
|
||||
|
||||
TEST(Angle8, Negate) {
|
||||
float angle = 0;
|
||||
Angle8 a = Angle8::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 90.0F;
|
||||
a = Angle8::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
|
||||
}
|
||||
|
||||
TEST(Angle8, Add) {
|
||||
Angle8 a = Angle8::Degrees(-45);
|
||||
Angle8 b = Angle8::Degrees(45.0F);
|
||||
Angle8 r = a + b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
|
||||
}
|
||||
|
||||
TEST(Angle8, Subtract) {
|
||||
Angle8 a = Angle8::Degrees(0);
|
||||
Angle8 b = Angle8::Degrees(45.0F);
|
||||
Angle8 r = a - b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
|
||||
}
|
||||
|
||||
TEST(Angle8, Compare) {
|
||||
Angle8 a = Angle8::Degrees(45);
|
||||
bool r = false;
|
||||
|
||||
r = a > Angle8::Degrees(0);
|
||||
EXPECT_TRUE(r) << "45 > 0";
|
||||
|
||||
r = a > Angle8::Degrees(90);
|
||||
EXPECT_FALSE(r) << "45 > 90";
|
||||
|
||||
r = a > Angle8::Degrees(-90);
|
||||
EXPECT_TRUE(r) << "45 > -90";
|
||||
}
|
||||
|
||||
TEST(Angle8, Normalize) {
|
||||
Angle8 r = Angle8();
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(90.0f));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(-90));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(270));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(270 + 360));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(-270));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(-270 - 360));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
|
||||
|
||||
if (false) { // std::numeric_limits<float>::is_iec559) {
|
||||
// Infinites are not supported
|
||||
r = Angle8::Normalize(Angle8::Degrees(FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
|
||||
|
||||
r = Angle8::Normalize(Angle8::Degrees(-FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Angle8, Clamp) {
|
||||
Angle8 r = Angle8();
|
||||
|
||||
// Clamp(1, 0, 2) will fail because Angle8 does not have enough resolution for
|
||||
// this. Instead we use Clamp(10, 0, 20) etc.
|
||||
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
|
||||
Angle8::Degrees(20));
|
||||
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 10 0 20";
|
||||
|
||||
r = Angle8::Clamp(Angle8::Degrees(-10), Angle8::Degrees(0),
|
||||
Angle8::Degrees(20));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -10 0 20";
|
||||
|
||||
r = Angle8::Clamp(Angle8::Degrees(30), Angle8::Degrees(0),
|
||||
Angle8::Degrees(20));
|
||||
EXPECT_NEAR(r.InDegrees(), 20, 1.0e-0) << "Clamp 30 0 20";
|
||||
|
||||
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
|
||||
Angle8::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 10 0 0";
|
||||
|
||||
r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(0), Angle8::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
|
||||
|
||||
r = Angle8::Clamp(Angle8::Degrees(0), Angle8::Degrees(10),
|
||||
Angle8::Degrees(-10));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
|
||||
|
||||
if (false) { // std::numeric_limits<float>::is_iec559) {
|
||||
// Infinites are not supported
|
||||
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
|
||||
Angle8::Degrees(FLOAT_INFINITY));
|
||||
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 0 INFINITY";
|
||||
|
||||
r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(-FLOAT_INFINITY),
|
||||
Angle8::Degrees(10));
|
||||
EXPECT_NEAR(r.InDegrees(), 10, 1.0e-0) << "Clamp 1 -INFINITY 1";
|
||||
}
|
||||
}
|
||||
|
||||
// TEST(Angle8, Difference) {
|
||||
// Angle8 r = 0;
|
||||
|
||||
// r = Angle8::Difference(0, 90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
|
||||
|
||||
// r = Angle8::Difference(0, -90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
|
||||
|
||||
// r = Angle8::Difference(0, 270);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
|
||||
|
||||
// r = Angle8::Difference(0, -270);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
|
||||
|
||||
// r = Angle8::Difference(90, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
|
||||
|
||||
// r = Angle8::Difference(-90, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
|
||||
|
||||
// r = Angle8::Difference(0, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
|
||||
|
||||
// r = Angle8::Difference(90, 90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
|
||||
|
||||
// if (std::numeric_limits<float>::is_iec559) {
|
||||
// r = Angle8::Difference(0, INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
|
||||
|
||||
// r = Angle8::Difference(0, -INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
|
||||
|
||||
// r = Angle8::Difference(-INFINITY, INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
|
||||
// INFINITY";
|
||||
// }
|
||||
// }
|
||||
|
||||
TEST(Angle8, MoveTowards) {
|
||||
Angle8 r = Angle8();
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 30);
|
||||
EXPECT_NEAR(r.InDegrees(), 30, 1.0e-0) << "MoveTowards 0 90 30";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 90);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), 180);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -180";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 270);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -90);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -180);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90), -270);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90), 0);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 0);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
|
||||
|
||||
if (false) { // std::numeric_limits<float>::is_iec559) {
|
||||
// infinites are not supported
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90),
|
||||
FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(FLOAT_INFINITY),
|
||||
30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(-90),
|
||||
-FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
|
||||
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
|
||||
|
||||
r = Angle8::MoveTowards(Angle8::Degrees(0),
|
||||
Angle8::Degrees(-FLOAT_INFINITY), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,249 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
using namespace LinearAlgebra;
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(AngleSingle, Construct) {
|
||||
float angle = 0.0F;
|
||||
AngleSingle a = AngleSingle::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = -180.0F;
|
||||
a = AngleSingle::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 270.0F;
|
||||
a = AngleSingle::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -90);
|
||||
}
|
||||
|
||||
TEST(AngleSingle, Negate) {
|
||||
float angle = 0;
|
||||
AngleSingle a = AngleSingle::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 90.0F;
|
||||
a = AngleSingle::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
|
||||
}
|
||||
|
||||
TEST(AngleSingle, Add) {
|
||||
AngleSingle a = AngleSingle::Degrees(-45);
|
||||
AngleSingle b = AngleSingle::Degrees(45.0F);
|
||||
AngleSingle r = a + b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
|
||||
}
|
||||
|
||||
TEST(AngleSingle, Subtract) {
|
||||
AngleSingle a = AngleSingle::Degrees(0);
|
||||
AngleSingle b = AngleSingle::Degrees(45.0F);
|
||||
AngleSingle r = a - b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
|
||||
}
|
||||
|
||||
TEST(AngleSingle, Compare) {
|
||||
AngleSingle a = AngleSingle::Degrees(45);
|
||||
bool r = false;
|
||||
|
||||
r = a > AngleSingle::Degrees(0);
|
||||
EXPECT_TRUE(r) << "45 > 0";
|
||||
|
||||
r = a > AngleSingle::Degrees(90);
|
||||
EXPECT_FALSE(r) << "45 > 90";
|
||||
|
||||
r = a > AngleSingle::Degrees(-90);
|
||||
EXPECT_TRUE(r) << "45 > -90";
|
||||
}
|
||||
|
||||
TEST(AngleSingle, Normalize) {
|
||||
AngleSingle r = AngleSingle();
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(90.0f));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(-90));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(270));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(270 + 360));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(-270));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(-270 - 360));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
|
||||
|
||||
r = AngleSingle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(AngleSingle, Clamp) {
|
||||
AngleSingle r = AngleSingle();
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(2));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(2));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(2));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1),
|
||||
AngleSingle::Degrees(-1));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 1 -1";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(FLOAT_INFINITY));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";
|
||||
|
||||
r = AngleSingle::Clamp(AngleSingle::Degrees(1),
|
||||
AngleSingle::Degrees(-FLOAT_INFINITY),
|
||||
AngleSingle::Degrees(1));
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
|
||||
}
|
||||
}
|
||||
|
||||
// TEST(AngleSingle, Difference) {
|
||||
// AngleSingle r = 0;
|
||||
|
||||
// r = AngleSingle::Difference(0, 90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
|
||||
|
||||
// r = AngleSingle::Difference(0, -90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
|
||||
|
||||
// r = AngleSingle::Difference(0, 270);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
|
||||
|
||||
// r = AngleSingle::Difference(0, -270);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
|
||||
|
||||
// r = AngleSingle::Difference(90, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
|
||||
|
||||
// r = AngleSingle::Difference(-90, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
|
||||
|
||||
// r = AngleSingle::Difference(0, 0);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
|
||||
|
||||
// r = AngleSingle::Difference(90, 90);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
|
||||
|
||||
// if (std::numeric_limits<float>::is_iec559) {
|
||||
// r = AngleSingle::Difference(0, INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
|
||||
|
||||
// r = AngleSingle::Difference(0, -INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
|
||||
|
||||
// r = AngleSingle::Difference(-INFINITY, INFINITY);
|
||||
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
|
||||
// INFINITY";
|
||||
// }
|
||||
// }
|
||||
|
||||
TEST(AngleSingle, MoveTowards) {
|
||||
AngleSingle r = AngleSingle();
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), 30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), 90);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), 180);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), 270);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 -30";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(-90), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -30";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(-90), -90);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -90";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(-90), -180);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -180";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(-90), -270);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -270";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), 0);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
|
||||
0);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
|
||||
30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(90), FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(FLOAT_INFINITY), 30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(-90), -FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -90 -FLOAT_INFINITY";
|
||||
|
||||
r = AngleSingle::MoveTowards(AngleSingle::Degrees(0),
|
||||
AngleSingle::Degrees(-FLOAT_INFINITY), -30);
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 -FLOAT_INFINITY -30";
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,56 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "Direction.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Direction16, Compare) {
|
||||
Direction16 d = Direction16::Degrees(45, 135);
|
||||
bool r;
|
||||
r = (d == Direction16(Angle16::Degrees(45), Angle16::Degrees(135)));
|
||||
EXPECT_TRUE(r) << "45,135 == 45, 135";
|
||||
|
||||
r = (d ==
|
||||
Direction16(Angle16::Degrees(45 + 360), Angle16::Degrees(135 - 360)));
|
||||
EXPECT_TRUE(r) << "45+360, 135-360 == 45, 135";
|
||||
}
|
||||
|
||||
TEST(Direction16, Inverse) {
|
||||
Direction16 d;
|
||||
Direction16 r;
|
||||
|
||||
d = Direction16::Degrees(45, 135);
|
||||
r = -d;
|
||||
EXPECT_EQ(r, Direction16::Degrees(-135, -135)) << "-(45, 135)";
|
||||
|
||||
d = Direction16::Degrees(-45, -135);
|
||||
r = -d;
|
||||
EXPECT_EQ(r, Direction16::Degrees(135, 135)) << "-(-45, -135)";
|
||||
|
||||
d = Direction16::Degrees(0, 0);
|
||||
r = -d;
|
||||
EXPECT_EQ(r, Direction16::Degrees(180, 0)) << "-(0, 0)";
|
||||
|
||||
d = Direction16::Degrees(0, 45);
|
||||
r = -d;
|
||||
EXPECT_EQ(r, Direction16::Degrees(180, -45)) << "-(0, 45)";
|
||||
}
|
||||
|
||||
TEST(Direction16, Equality) {
|
||||
Direction16 d;
|
||||
d = Direction16::Degrees(135, 45);
|
||||
EXPECT_EQ(d, Direction16::Degrees(135, 45)) << "(135, 45) == (135, 45)";
|
||||
EXPECT_EQ(d, Direction16::Degrees(135 + 360, 45))
|
||||
<< "(135, 45) == (135 + 360, 45) ";
|
||||
EXPECT_EQ(d, Direction16::Degrees(135 - 360, 45))
|
||||
<< "(135, 135) == (135 - 360, 45) ";
|
||||
|
||||
d = Direction16::Degrees(0, 45 + 180);
|
||||
EXPECT_EQ(d, Direction16::Degrees(180, -45)) << "(0, 45+180) == (180, -45)";
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,82 +0,0 @@
|
||||
/*
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "Angle.h"
|
||||
// #include "Angle16.h"
|
||||
// #include "Angle8.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Angle8, Construct) {
|
||||
float angle = 0.0F;
|
||||
Angle8 a = Angle8::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = -180.0F;
|
||||
a = Angle8::Degrees(angle);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
}
|
||||
|
||||
TEST(Angle8, Negate) {
|
||||
float angle = 0;
|
||||
Angle8 a = Angle8::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 90.0F;
|
||||
a = Angle8::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
|
||||
}
|
||||
|
||||
TEST(Angle8, Add) {
|
||||
Angle8 a = Angle8::Degrees(-45);
|
||||
Angle8 b = Angle8::Degrees(45.0F);
|
||||
Angle8 r = a + b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
|
||||
}
|
||||
|
||||
TEST(Angle8, Subtract) {
|
||||
Angle8 a = Angle8::Degrees(0);
|
||||
Angle8 b = Angle8::Degrees(45.0F);
|
||||
Angle8 r = a - b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
|
||||
}
|
||||
|
||||
TEST(Angle16, Construct) {
|
||||
Angle16 a = Angle16::Degrees(0.0F);
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), 0);
|
||||
}
|
||||
|
||||
TEST(Angle16, Negate) {
|
||||
float angle = 0;
|
||||
Angle16 a = Angle16::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), angle);
|
||||
|
||||
angle = 90.0F;
|
||||
a = Angle16::Degrees(angle);
|
||||
a = -a;
|
||||
EXPECT_FLOAT_EQ(a.InDegrees(), -angle);
|
||||
}
|
||||
|
||||
TEST(Angle16, Subtract) {
|
||||
Angle16 a = Angle16::Degrees(0);
|
||||
Angle16 b = Angle16::Degrees(45.0F);
|
||||
Angle16 r = a - b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), -45);
|
||||
}
|
||||
|
||||
TEST(Angle16, Add) {
|
||||
Angle16 a = Angle16::Degrees(-45);
|
||||
Angle16 b = Angle16::Degrees(45.0F);
|
||||
Angle16 r = a + b;
|
||||
EXPECT_FLOAT_EQ(r.InDegrees(), 0);
|
||||
}
|
||||
|
||||
#endif
|
||||
*/
|
||||
@ -1,41 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "FloatSingle.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(FloatC, Clamp) {
|
||||
float r = 0;
|
||||
|
||||
r = Float::Clamp(1, 0, 2);
|
||||
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2";
|
||||
|
||||
r = Float::Clamp(-1, 0, 2);
|
||||
EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2";
|
||||
|
||||
r = Float::Clamp(3, 0, 2);
|
||||
EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2";
|
||||
|
||||
r = Float::Clamp(1, 0, 0);
|
||||
EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0";
|
||||
|
||||
r = Float::Clamp(0, 0, 0);
|
||||
EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0";
|
||||
|
||||
r = Float::Clamp(0, 1, -1);
|
||||
EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 1 -1";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
r = Float::Clamp(1, 0, FLOAT_INFINITY);
|
||||
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY";
|
||||
|
||||
r = Float::Clamp(1, -FLOAT_INFINITY, 1);
|
||||
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1";
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,135 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "Matrix.h"
|
||||
|
||||
TEST(MatrixSingle, Init) {
|
||||
// zero
|
||||
MatrixOf<float> m0 = MatrixOf<float>(0, 0);
|
||||
|
||||
// one
|
||||
float data1[] = {1.0F};
|
||||
MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1);
|
||||
|
||||
// two
|
||||
float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
|
||||
MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2);
|
||||
|
||||
// negative
|
||||
// MatrixOf<float> m_1 = MatrixOf<float>(-1, -1);
|
||||
// parameters are unsigned
|
||||
}
|
||||
|
||||
TEST(MatrixSingle, Transpose) {
|
||||
float data1[] = {1.0F};
|
||||
MatrixOf<float> m = MatrixOf<float>(1, 1, data1);
|
||||
|
||||
MatrixOf<float> r = MatrixOf<float>(1, 1);
|
||||
m.Transpose(&r);
|
||||
|
||||
// 2 x 2
|
||||
|
||||
float data3[] = {1.0F, 2.0F, 3.0F, 4.0F};
|
||||
MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3);
|
||||
EXPECT_EQ(m22.RowCount(), 2);
|
||||
EXPECT_EQ(m22.ColCount(), 2);
|
||||
|
||||
float data4[] = {0.0F, 0.0F, 0.0F, 0.0F};
|
||||
MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4);
|
||||
EXPECT_EQ(r22.RowCount(), 2);
|
||||
EXPECT_EQ(r22.ColCount(), 2);
|
||||
|
||||
m22.Transpose(&r22);
|
||||
EXPECT_EQ(r22.RowCount(), 2);
|
||||
EXPECT_EQ(r22.ColCount(), 2);
|
||||
EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.0F);
|
||||
EXPECT_FLOAT_EQ(r22.Get(0, 1), 3.0F);
|
||||
EXPECT_FLOAT_EQ(r22.Get(1, 0), 2.0F);
|
||||
EXPECT_FLOAT_EQ(r22.Get(1, 1), 4.0F);
|
||||
|
||||
// 1 x 2
|
||||
float data12[] = {1.0F, 2.0F};
|
||||
MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12);
|
||||
EXPECT_EQ(m12.RowCount(), 1);
|
||||
EXPECT_EQ(m12.ColCount(), 2);
|
||||
|
||||
float data21[] = {0.0F, 0.0F};
|
||||
MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21);
|
||||
EXPECT_EQ(r21.RowCount(), 2);
|
||||
EXPECT_EQ(r21.ColCount(), 1);
|
||||
|
||||
m12.Transpose(&r21);
|
||||
EXPECT_EQ(r21.RowCount(), 2);
|
||||
EXPECT_EQ(r21.ColCount(), 1);
|
||||
EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F);
|
||||
EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F);
|
||||
|
||||
// changing dimensions, same size is okay
|
||||
MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21);
|
||||
EXPECT_EQ(r12.RowCount(), 1);
|
||||
EXPECT_EQ(r12.ColCount(), 2);
|
||||
|
||||
m12.Transpose(&r12);
|
||||
EXPECT_EQ(r12.RowCount(), 2);
|
||||
EXPECT_EQ(r12.ColCount(), 1);
|
||||
EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F);
|
||||
EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F);
|
||||
}
|
||||
|
||||
TEST(MatrixSingle, Multiply) {
|
||||
float m12data[] = {1.0F, 2.0F};
|
||||
MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data);
|
||||
|
||||
EXPECT_EQ(m12.RowCount(), 1);
|
||||
EXPECT_EQ(m12.ColCount(), 2);
|
||||
EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F);
|
||||
EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F);
|
||||
|
||||
float m21data[] = {3.0F, 4.0F};
|
||||
MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data);
|
||||
|
||||
EXPECT_EQ(m21.RowCount(), 2);
|
||||
EXPECT_EQ(m21.ColCount(), 1);
|
||||
EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F);
|
||||
EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F);
|
||||
|
||||
float r11data[] = {0.0F};
|
||||
MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data);
|
||||
|
||||
EXPECT_EQ(r11.RowCount(), 1);
|
||||
EXPECT_EQ(r11.ColCount(), 1);
|
||||
|
||||
MatrixOf<float>::Multiply(&m12, &m21, &r11);
|
||||
EXPECT_EQ(r11.RowCount(), 1);
|
||||
EXPECT_EQ(r11.ColCount(), 1);
|
||||
EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F);
|
||||
|
||||
float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F};
|
||||
MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data);
|
||||
|
||||
MatrixOf<float>::Multiply(&m21, &m12, &r22);
|
||||
EXPECT_EQ(r22.RowCount(), 2);
|
||||
EXPECT_EQ(r22.ColCount(), 2);
|
||||
EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.0F);
|
||||
EXPECT_FLOAT_EQ(r22.Get(0, 1), 4.0F);
|
||||
EXPECT_FLOAT_EQ(r22.Get(1, 0), 6.0F);
|
||||
EXPECT_FLOAT_EQ(r22.Get(1, 1), 8.0F);
|
||||
}
|
||||
|
||||
TEST(MatrixSingle, Multiply_Vector3) {
|
||||
Vector3 v = Vector3(1.0, 2.0, 3.0);
|
||||
Vector3 r = Vector3::zero;
|
||||
|
||||
// float m13data[] = {3.0, 4.0, 5.0};
|
||||
// MatrixOf<float> m13 = MatrixOf<float>(1, 3, m13data);
|
||||
// Vector3 r = MatrixOf<float>::Multiply(&m13, v);
|
||||
|
||||
float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
MatrixOf<float> m33 = MatrixOf<float>(3, 3, m33data);
|
||||
r = MatrixOf<float>::Multiply(&m33, v);
|
||||
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,232 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "Polar.h"
|
||||
#include "Spherical.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Polar, FromVector2) {
|
||||
Vector2 v = Vector2(0, 1);
|
||||
PolarSingle p = PolarSingle::FromVector2(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 0 1";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "s.angle 0 0 1";
|
||||
|
||||
v = Vector2(1, 0);
|
||||
p = PolarSingle::FromVector2(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 1 0";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 90.0F) << "s.angle 1 0";
|
||||
|
||||
v = Vector2(-1, 1);
|
||||
p = PolarSingle::FromVector2(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, sqrt(2.0F)) << "p.distance -1 1";
|
||||
EXPECT_NEAR(p.angle.InDegrees(), -45.0F, 1.0e-05) << "s.angle -1 1";
|
||||
}
|
||||
|
||||
TEST(Polar, FromSpherical) {
|
||||
SphericalSingle s;
|
||||
PolarSingle p;
|
||||
|
||||
s = SphericalSingle(1, DirectionSingle::forward);
|
||||
p = PolarSingle::FromSpherical(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 0 0)";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(1 0 0)";
|
||||
|
||||
s = SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0));
|
||||
p = PolarSingle::FromSpherical(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 45 0)";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 45.0F)
|
||||
<< "p.angle FromSpherical(1 45 0)";
|
||||
|
||||
s = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0));
|
||||
p = PolarSingle::FromSpherical(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 -45 0)";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), -45.0F)
|
||||
<< "p.angle FromSpherical(1 -45 0)";
|
||||
|
||||
s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(0));
|
||||
p = PolarSingle::FromSpherical(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 0)";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 0)";
|
||||
|
||||
s = SphericalSingle(-1, AngleSingle::Degrees(0), AngleSingle::Degrees(0));
|
||||
p = PolarSingle::FromSpherical(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(-1 0 0)";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), -180.0F)
|
||||
<< "p.angle FromSpherical(-1 0 0)";
|
||||
|
||||
s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(90));
|
||||
p = PolarSingle::FromSpherical(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 90)";
|
||||
EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 90)";
|
||||
}
|
||||
|
||||
TEST(Polar, Negation) {
|
||||
PolarSingle v = PolarSingle(2, AngleSingle::Degrees(45));
|
||||
PolarSingle r = PolarSingle::zero;
|
||||
|
||||
r = -v;
|
||||
EXPECT_FLOAT_EQ(r.distance, 2);
|
||||
EXPECT_FLOAT_EQ(r.angle.InDegrees(), -135);
|
||||
EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(-135)))
|
||||
<< "Negate(2 45)";
|
||||
|
||||
v = PolarSingle::Deg(2, -45);
|
||||
r = -v;
|
||||
EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(135)))
|
||||
<< "Negate(2 -45)";
|
||||
|
||||
v = PolarSingle::Degrees(2, 0);
|
||||
r = -v;
|
||||
EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(180))) << "Negate(2 0)";
|
||||
|
||||
v = PolarSingle(0, AngleSingle::Degrees(0));
|
||||
r = -v;
|
||||
EXPECT_FLOAT_EQ(r.distance, 0.0f);
|
||||
EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0.0f);
|
||||
EXPECT_TRUE(r == PolarSingle(0, AngleSingle::Degrees(0))) << "Negate(0 0)";
|
||||
}
|
||||
|
||||
TEST(Polar, Subtraction) {
|
||||
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
|
||||
PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
|
||||
PolarSingle r = PolarSingle::zero;
|
||||
|
||||
r = v1 - v2;
|
||||
// don't know what to expect yet
|
||||
|
||||
v2 = PolarSingle::zero;
|
||||
r = v1 - v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Subtraction(0 0)";
|
||||
}
|
||||
|
||||
TEST(Polar, Addition) {
|
||||
PolarSingle v1 = PolarSingle(1, AngleSingle::Degrees(45));
|
||||
PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
|
||||
PolarSingle r = PolarSingle::zero;
|
||||
|
||||
r = v1 - v2;
|
||||
// don't know what to expect yet
|
||||
|
||||
v2 = PolarSingle::zero;
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
|
||||
|
||||
r = v1;
|
||||
r += v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
|
||||
|
||||
v2 = PolarSingle(1, AngleSingle::Degrees(-45));
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)";
|
||||
EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0) << "Addition(0 0 0)";
|
||||
}
|
||||
|
||||
TEST(Polar, Scale_Multiply) {
|
||||
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
|
||||
PolarSingle r = PolarSingle::zero;
|
||||
|
||||
r = v1 * 2.0f;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance * 2) << "ScaleMult(4 45, 2)";
|
||||
EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees())
|
||||
<< "ScaleMult(4 45, 2)";
|
||||
}
|
||||
|
||||
TEST(Polar, Scale_Divide) {
|
||||
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
|
||||
PolarSingle r = PolarSingle::zero;
|
||||
|
||||
r = v1 / 2.0f;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance / 2) << "ScaleDiv(4 45, 2)";
|
||||
EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees())
|
||||
<< "ScaleDiv(4 45, 2)";
|
||||
}
|
||||
|
||||
TEST(Polar, Distance) {
|
||||
PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
|
||||
PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
|
||||
float d = 0;
|
||||
|
||||
d = PolarSingle::Distance(v1, v2);
|
||||
// don't know what to expect yet
|
||||
|
||||
v2 = PolarSingle::zero;
|
||||
d = PolarSingle::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(d, v1.distance) << "Distance(4 45, zero)";
|
||||
}
|
||||
|
||||
TEST(Polar, Rotate) {
|
||||
PolarSingle v = PolarSingle(4, AngleSingle::Degrees(45));
|
||||
PolarSingle r = PolarSingle::zero;
|
||||
|
||||
r = PolarSingle::Rotate(v, AngleSingle::Degrees(45));
|
||||
EXPECT_FLOAT_EQ(r.distance, v.distance) << "Rotate(4 45, 45)";
|
||||
EXPECT_FLOAT_EQ(r.angle.InDegrees(), 90.0f) << "Rotate(4 45, 45)";
|
||||
}
|
||||
|
||||
// Performance Test
|
||||
TEST(PolarOfTest, PerformanceTest) {
|
||||
const int numIterations = 1000000; // Number of instances to test
|
||||
std::vector<PolarOf<float>> polarObjects;
|
||||
|
||||
// Measure time for creating a large number of PolarOf objects
|
||||
auto start = std::chrono::high_resolution_clock::now();
|
||||
|
||||
for (int i = 0; i < numIterations; ++i) {
|
||||
float distance =
|
||||
static_cast<float>(rand() % 100); // Random distance from 0 to 100
|
||||
AngleOf<float> angle = AngleOf<float>::Degrees(
|
||||
static_cast<float>(rand() % 360)); // Random angle from 0 to 360 degrees
|
||||
PolarOf<float> p = PolarOf<float>(distance, angle);
|
||||
polarObjects.emplace_back(p); // Create and store the object
|
||||
}
|
||||
|
||||
auto end = std::chrono::high_resolution_clock::now();
|
||||
std::chrono::duration<double> duration = end - start;
|
||||
std::cout << "Time to construct " << numIterations
|
||||
<< " PolarOf objects: " << duration.count() << " seconds."
|
||||
<< std::endl;
|
||||
|
||||
// Test completion with a message
|
||||
ASSERT_GE(duration.count(), 0); // Ensure duration is non-negative
|
||||
|
||||
// Assert that the duration is less than or equal to 1 second
|
||||
ASSERT_LE(duration.count(), 1.0)
|
||||
<< "Performance test failed: Construction took longer than 1 second.";
|
||||
}
|
||||
|
||||
// Edge Case 1: Testing with distance = 0 and angle = 45
|
||||
TEST(PolarOfTest, TestDistanceZero) {
|
||||
PolarOf<float> p1(0.0f, AngleOf<float>::Degrees(45.0f));
|
||||
EXPECT_EQ(p1.distance, 0.0f); // Ensure distance is 0
|
||||
EXPECT_EQ(p1.angle.InDegrees(), 0.0f); // Ensure angle is 0 when distance is 0
|
||||
}
|
||||
|
||||
// Edge Case 2: Testing with negative distance, angle should be adjusted
|
||||
TEST(PolarOfTest, TestNegativeDistance) {
|
||||
PolarOf<float> p2(-10.0f, AngleOf<float>::Degrees(90.0f));
|
||||
EXPECT_EQ(p2.distance, 10.0f); // Ensure distance is positive
|
||||
EXPECT_NEAR(p2.angle.InDegrees(), -90.0f,
|
||||
0.0001f); // Ensure angle is normalized to 270 degrees (180 + 90)
|
||||
}
|
||||
|
||||
// Edge Case 3: Testing with positive distance and angle = 180
|
||||
TEST(PolarOfTest, TestPositiveDistance) {
|
||||
PolarOf<float> p3(100.0f, AngleOf<float>::Degrees(180.0f));
|
||||
EXPECT_EQ(p3.distance, 100.0f); // Ensure distance is correct
|
||||
EXPECT_NEAR(p3.angle.InDegrees(), -180.0f,
|
||||
0.0001f); // Ensure angle is correct
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,190 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "Quaternion.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Quaternion, Normalize) {
|
||||
bool r = false;
|
||||
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
Quaternion q = Quaternion::identity;
|
||||
|
||||
q = q1;
|
||||
q.Normalize();
|
||||
r = q == q1;
|
||||
EXPECT_TRUE(r) << "q.Normalzed 0 0 0 1";
|
||||
|
||||
q = Quaternion::Normalize(q1);
|
||||
r = q == q1;
|
||||
EXPECT_TRUE(r) << "Quaternion::Normalize 0 0 0 1";
|
||||
}
|
||||
|
||||
TEST(Quaternion, ToAngles) {
|
||||
bool r = false;
|
||||
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = Quaternion::ToAngles(q1);
|
||||
r = v == Vector3(0, 0, 0);
|
||||
EXPECT_TRUE(r) << "Quaternion::ToAngles 0 0 0 1";
|
||||
|
||||
q1 = Quaternion(1, 0, 0, 0);
|
||||
v = Quaternion::ToAngles(q1);
|
||||
r = v == Vector3(180, 0, 0);
|
||||
// EXPECT_TRUE(r) << "Quaternion::ToAngles 1 0 0 0";
|
||||
// fails on MacOS?
|
||||
}
|
||||
|
||||
TEST(Quaternion, Multiplication) {
|
||||
bool r = false;
|
||||
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
Quaternion q2 = Quaternion(1, 0, 0, 0);
|
||||
Quaternion q = Quaternion::identity;
|
||||
|
||||
q = q1 * q2;
|
||||
r = q == Quaternion(1, 0, 0, 0);
|
||||
EXPECT_TRUE(r) << "0 0 0 1 * 1 0 0 0";
|
||||
}
|
||||
|
||||
TEST(Quaternion, MultiplicationVector) {
|
||||
bool r = false;
|
||||
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
Vector3 v1 = Vector3(0, 1, 0);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = q1 * v1;
|
||||
r = v == Vector3(0, 1, 0);
|
||||
EXPECT_TRUE(r) << "0 0 0 1 * Vector 0 1 0";
|
||||
|
||||
q1 = Quaternion(1, 0, 0, 0);
|
||||
v = q1 * v1;
|
||||
r = v == Vector3(0, -1, 0);
|
||||
EXPECT_TRUE(r) << "1 0 0 0 * Vector 0 1 0";
|
||||
}
|
||||
|
||||
TEST(Quaternion, Equality) {
|
||||
bool r = false;
|
||||
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
Quaternion q2 = Quaternion(1, 0, 0, 0);
|
||||
|
||||
r = q1 == q2;
|
||||
EXPECT_FALSE(r) << " 0 0 0 1 == 1 0 0 0";
|
||||
|
||||
q2 = Quaternion(0, 0, 0, 1);
|
||||
r = q1 == q2;
|
||||
EXPECT_TRUE(r) << "0 0 0 1 == 0 0 0 1";
|
||||
}
|
||||
|
||||
TEST(Quaternion, Inverse) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, LookRotation) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, FromToRotation) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, RotateTowards) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, AngleAxis) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, Angle) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, Slerp) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, SlerpUnclamped) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, Euler) {
|
||||
bool r = false;
|
||||
|
||||
Vector3 v1 = Vector3(0, 0, 0);
|
||||
Quaternion q = Quaternion::identity;
|
||||
|
||||
q = Quaternion::Euler(v1);
|
||||
r = q == Quaternion::identity;
|
||||
EXPECT_TRUE(r) << "Euler Vector 0 0 0";
|
||||
|
||||
q = Quaternion::Euler(0, 0, 0);
|
||||
r = q == Quaternion::identity;
|
||||
EXPECT_TRUE(r) << "Euler 0 0 0";
|
||||
|
||||
v1 = Vector3(90, 90, -90);
|
||||
q = Quaternion::Euler(v1);
|
||||
r = q == Quaternion(0, 0.707106709F, -0.707106709F, 0);
|
||||
EXPECT_TRUE(r) << "Euler Vector 90 90 -90";
|
||||
|
||||
q = Quaternion::Euler(90, 90, -90);
|
||||
r = q == Quaternion(0, 0.707106709F, -0.707106709F, 0);
|
||||
EXPECT_TRUE(r) << "Euler 90 90 -90";
|
||||
}
|
||||
|
||||
TEST(Quaternion, GetAngleAround) {
|
||||
bool r = false;
|
||||
|
||||
Vector3 v1 = Vector3(0, 1, 0);
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
float f;
|
||||
|
||||
f = Quaternion::GetAngleAround(v1, q1);
|
||||
EXPECT_FLOAT_EQ(f, 0) << "GetAngleAround 0 1 0 , 0 0 0 1";
|
||||
|
||||
q1 = Quaternion(0, 0.707106709F, -0.707106709F, 0);
|
||||
f = Quaternion::GetAngleAround(v1, q1);
|
||||
EXPECT_FLOAT_EQ(f, 180) << "GetAngleAround 0 1 0 , 0 0.7 -0.7 0";
|
||||
|
||||
v1 = Vector3(0, 0, 0);
|
||||
f = Quaternion::GetAngleAround(v1, q1);
|
||||
r = isnan(f);
|
||||
EXPECT_TRUE(r) << "GetAngleAround 0 0 0 , 0 0.7 -0.7 0";
|
||||
}
|
||||
|
||||
TEST(Quaternion, GetRotationAround) {
|
||||
bool r = false;
|
||||
|
||||
Vector3 v1 = Vector3(0, 1, 0);
|
||||
Quaternion q1 = Quaternion(0, 0, 0, 1);
|
||||
Quaternion q = Quaternion::identity;
|
||||
|
||||
q = Quaternion::GetRotationAround(v1, q1);
|
||||
r = q == Quaternion::identity;
|
||||
EXPECT_TRUE(r) << "GetRotationAround 0 1 0 , 0 0 0 1";
|
||||
|
||||
q1 = Quaternion(0, 0.707106709F, -0.707106709F, 0);
|
||||
q = Quaternion::GetRotationAround(v1, q1);
|
||||
r = q == Quaternion(0, 1, 0, 0);
|
||||
EXPECT_TRUE(r) << "GetRotationAround 0 1 0 , 0 0.7 -0.7 0";
|
||||
|
||||
v1 = Vector3(0, 0, 0);
|
||||
q = Quaternion::GetRotationAround(v1, q1);
|
||||
r = isnan(q.x) && isnan(q.y) && isnan(q.z) && isnan(q.w);
|
||||
EXPECT_TRUE(r) << "GetRotationAround 0 0 0 , 0 0.7 -0.7 0";
|
||||
}
|
||||
|
||||
TEST(Quaternion, GetSwingTwist) {
|
||||
|
||||
}
|
||||
|
||||
TEST(Quaternion, Dot) {
|
||||
}
|
||||
#endif
|
||||
@ -1,222 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "Spherical.h"
|
||||
#include "Vector3.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Spherical16, FromVector3) {
|
||||
Vector3 v = Vector3(0, 0, 1);
|
||||
Spherical16 s = Spherical16::FromVector3(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
|
||||
EXPECT_FLOAT_EQ((float)s.direction.horizontal.InDegrees(), 0.0F)
|
||||
<< "s.hor 0 0 1";
|
||||
EXPECT_FLOAT_EQ((float)s.direction.vertical.InDegrees(), 0.0F)
|
||||
<< "s.vert 0 0 1";
|
||||
|
||||
v = Vector3(0, 1, 0);
|
||||
s = Spherical16::FromVector3(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
|
||||
|
||||
v = Vector3(1, 0, 0);
|
||||
s = Spherical16::FromVector3(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
|
||||
}
|
||||
|
||||
TEST(Spherical16, Vector3) {
|
||||
Vector3 v = Vector3(1, 2, 3);
|
||||
Spherical16 rd = Spherical16::FromVector3(v);
|
||||
Vector3 rv = rd.ToVector3();
|
||||
EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
|
||||
|
||||
v = Vector3(1, 2, -3);
|
||||
rd = Spherical16::FromVector3(v);
|
||||
rv = rd.ToVector3();
|
||||
EXPECT_LT(Vector3::Distance(v, rv), 10e-4) << " 1 2 3 <-> spherical";
|
||||
}
|
||||
|
||||
// TEST(Spherical16, FromPolar) {
|
||||
// Polar p = Polar(1, 0);
|
||||
// Spherical16 s = Spherical16::FromPolar(p);
|
||||
|
||||
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
|
||||
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(1 0)";
|
||||
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 0)";
|
||||
|
||||
// p = Polar(1, 45);
|
||||
// s = Spherical16::FromPolar(p);
|
||||
|
||||
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
|
||||
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 45.0F) << "s.hor Polar(1 45)";
|
||||
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 45)";
|
||||
|
||||
// p = Polar(1, -45);
|
||||
// s = Spherical16::FromPolar(p);
|
||||
|
||||
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
|
||||
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -45.0F) << "s.hor Polar(1 -45)";
|
||||
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(1 -45)";
|
||||
|
||||
// p = Polar(0, 0);
|
||||
// s = Spherical16::FromPolar(p);
|
||||
|
||||
// EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
|
||||
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), 0.0F) << "s.hor Polar(0 0)";
|
||||
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(0 0)";
|
||||
|
||||
// p = Polar(-1, 0);
|
||||
// s = Spherical16::FromPolar(p);
|
||||
|
||||
// EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
|
||||
// EXPECT_FLOAT_EQ(s.horizontal.InDegrees(), -180.0F) << "s.hor Polar(-1 0)";
|
||||
// EXPECT_FLOAT_EQ(s.vertical.InDegrees(), 0.0F) << "s.vert Polar(-1 0)";
|
||||
// }
|
||||
|
||||
TEST(Spherical16, Incident1) {
|
||||
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
|
||||
Spherical16 s = Spherical16::FromVector3(v);
|
||||
|
||||
Spherical16 sr =
|
||||
Spherical16(2.49F, Angle16::Degrees(98.18f), Angle16::Degrees(24.4F));
|
||||
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
|
||||
sr.direction.horizontal.InDegrees(), 1.0e-02);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(),
|
||||
sr.direction.vertical.InDegrees(), 1.0e-02);
|
||||
|
||||
Vector3 r =
|
||||
Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
|
||||
.ToVector3();
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
|
||||
}
|
||||
|
||||
TEST(Spherical16, Incident2) {
|
||||
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
|
||||
Spherical16 s = Spherical16::FromVector3(v);
|
||||
|
||||
Spherical16 sr = Spherical16(1.4142135623F, Angle16::Degrees(45.0f),
|
||||
Angle16::Degrees(0.0F));
|
||||
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
|
||||
sr.direction.horizontal.InDegrees(), 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(),
|
||||
sr.direction.vertical.InDegrees(), 1.0e-05);
|
||||
|
||||
Vector3 r =
|
||||
Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
|
||||
.ToVector3();
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
|
||||
|
||||
v = Vector3(0.0f, 1.0f, 1.0f);
|
||||
s = Spherical16::FromVector3(v);
|
||||
|
||||
sr = Spherical16(1.4142135623F, Angle16::Degrees(0), Angle16::Degrees(45));
|
||||
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
|
||||
sr.direction.horizontal.InDegrees(), 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(),
|
||||
sr.direction.vertical.InDegrees(), 1.0e-05);
|
||||
|
||||
r = Spherical16(sr.distance, sr.direction.horizontal, sr.direction.vertical)
|
||||
.ToVector3();
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
|
||||
|
||||
v = Vector3(1.0f, 1.0f, 1.0f);
|
||||
s = Spherical16::FromVector3(v);
|
||||
r = Spherical16(s.distance, s.direction.horizontal, s.direction.vertical)
|
||||
.ToVector3();
|
||||
|
||||
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
|
||||
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04);
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04);
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-04);
|
||||
|
||||
// s = Spherical16(10, 45, 45);
|
||||
// r = s.ToVector3();
|
||||
// EXPECT_NEAR(r.x, 5, 1.0e-06);
|
||||
// EXPECT_NEAR(r.y, 7.07, 1.0e-06);
|
||||
// EXPECT_NEAR(r.z, 5, 1.0e-06);
|
||||
}
|
||||
|
||||
TEST(Spherical16, Addition) {
|
||||
Spherical16 v1 = Spherical16(1, Angle16::Degrees(45), Angle16::Degrees(0));
|
||||
Spherical16 v2 = Spherical16::zero;
|
||||
Spherical16 r = Spherical16::zero;
|
||||
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
|
||||
|
||||
r = v1;
|
||||
r += v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
|
||||
|
||||
v2 = Spherical16(1, Angle16::Degrees(-45), Angle16::Degrees(0));
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
|
||||
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
|
||||
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
|
||||
|
||||
v2 = Spherical16(1, Angle16::Degrees(0), Angle16::Degrees(90));
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
|
||||
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
|
||||
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
|
||||
}
|
||||
|
||||
TEST(Spherical16, AdditionPerformance) {
|
||||
const int numIterations = 1000000; // Number of additions to test
|
||||
std::vector<Spherical16> sphericalObjects;
|
||||
|
||||
// Populate the vector with random SphericalOf objects
|
||||
for (int i = 0; i < numIterations; ++i) {
|
||||
float distance = (float)(rand() % 100);
|
||||
float horizontal = (float)(rand() % 180);
|
||||
float vertical = (float)(rand() % 360);
|
||||
Spherical16 s = Spherical16::Deg(distance, horizontal, vertical);
|
||||
sphericalObjects.push_back(s);
|
||||
}
|
||||
|
||||
// Measure the time to perform multiple additions
|
||||
auto start = std::chrono::high_resolution_clock::now();
|
||||
|
||||
Spherical16 result = Spherical16::zero; // Start with a
|
||||
// zero-initialized object
|
||||
|
||||
for (int i = 0; i < numIterations - 1; ++i) {
|
||||
result = result + sphericalObjects[i]; // Add objects
|
||||
// together
|
||||
}
|
||||
|
||||
auto end = std::chrono::high_resolution_clock::now();
|
||||
std::chrono::duration<double> duration = end - start;
|
||||
std::cout << "Time to perform " << numIterations - 1
|
||||
<< " additions: " << duration.count() << " seconds." << std::endl;
|
||||
|
||||
// Assert that the time taken is less than
|
||||
// 1 second (or any other performance
|
||||
// requirement)
|
||||
ASSERT_LE(duration.count(), 2.0) << "Performance test failed: "
|
||||
"Additions took longer than 1 "
|
||||
"second.";
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,213 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "Spherical.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(SphericalSingle, FromVector3) {
|
||||
Vector3 v = Vector3(0, 0, 1);
|
||||
SphericalSingle s = SphericalSingle ::FromVector3(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1";
|
||||
|
||||
v = Vector3(0, 1, 0);
|
||||
s = SphericalSingle ::FromVector3(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
|
||||
|
||||
v = Vector3(1, 0, 0);
|
||||
s = SphericalSingle ::FromVector3(v);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
|
||||
}
|
||||
|
||||
TEST(SphericalSingle, FromPolar) {
|
||||
PolarSingle p = PolarSingle(1, AngleSingle::Degrees(0));
|
||||
SphericalSingle s = SphericalSingle ::FromPolar(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
|
||||
<< "s.hor Polar(1 0)";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
|
||||
<< "s.vert Polar(1 0)";
|
||||
|
||||
p = PolarSingle(1, AngleSingle::Degrees(45));
|
||||
s = SphericalSingle ::FromPolar(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F)
|
||||
<< "s.hor Polar(1 45)";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
|
||||
<< "s.vert Polar(1 45)";
|
||||
|
||||
p = PolarSingle(1, AngleSingle::Degrees(-45));
|
||||
s = SphericalSingle ::FromPolar(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F)
|
||||
<< "s.hor Polar(1 -45)";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
|
||||
<< "s.vert Polar(1 -45)";
|
||||
|
||||
p = PolarSingle(0, AngleSingle::Degrees(0));
|
||||
s = SphericalSingle ::FromPolar(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
|
||||
<< "s.hor Polar(0 0)";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
|
||||
<< "s.vert Polar(0 0)";
|
||||
|
||||
p = PolarSingle(-1, AngleSingle::Degrees(0));
|
||||
s = SphericalSingle ::FromPolar(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
|
||||
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F)
|
||||
<< "s.hor Polar(-1 0)";
|
||||
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
|
||||
<< "s.vert Polar(-1 0)";
|
||||
}
|
||||
|
||||
TEST(SphericalSingle, Incident1) {
|
||||
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
|
||||
SphericalSingle s = SphericalSingle ::FromVector3(v);
|
||||
|
||||
SphericalSingle sr = SphericalSingle(2.49F, AngleSingle::Degrees(98.18f),
|
||||
AngleSingle::Degrees(24.4F));
|
||||
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
|
||||
sr.direction.horizontal.InDegrees(), 1.0e-02);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(),
|
||||
sr.direction.vertical.InDegrees(), 1.0e-02);
|
||||
|
||||
Vector3 r = Vector3(sr);
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
|
||||
}
|
||||
|
||||
TEST(SphericalSingle, Incident2) {
|
||||
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
|
||||
SphericalSingle s = SphericalSingle ::FromVector3(v);
|
||||
|
||||
SphericalSingle sr = SphericalSingle(
|
||||
1.4142135623F, AngleSingle::Degrees(45.0f), AngleSingle::Degrees(0.0F));
|
||||
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
|
||||
sr.direction.horizontal.InDegrees(), 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(),
|
||||
sr.direction.vertical.InDegrees(), 1.0e-05);
|
||||
|
||||
Vector3 r = Vector3(sr);
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
|
||||
|
||||
v = Vector3(0.0f, 1.0f, 1.0f);
|
||||
s = SphericalSingle ::FromVector3(v);
|
||||
|
||||
sr = SphericalSingle(1.4142135623F, AngleSingle::Degrees(0.0f),
|
||||
AngleSingle::Degrees(45.0F));
|
||||
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
|
||||
sr.direction.horizontal.InDegrees(), 1.0e-05);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(),
|
||||
sr.direction.vertical.InDegrees(), 1.0e-05);
|
||||
|
||||
r = Vector3(sr);
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
|
||||
|
||||
v = Vector3(1.0f, 1.0f, 1.0f);
|
||||
s = SphericalSingle ::FromVector3(v);
|
||||
r = Vector3(s);
|
||||
|
||||
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
|
||||
EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
|
||||
EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
|
||||
|
||||
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
|
||||
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
|
||||
|
||||
// s = SphericalSingle(10, 45, 45);
|
||||
// r = s.ToVector3();
|
||||
// EXPECT_NEAR(r.x, 5, 1.0e-06);
|
||||
// EXPECT_NEAR(r.y, 7.07, 1.0e-06);
|
||||
// EXPECT_NEAR(r.z, 5, 1.0e-06);
|
||||
}
|
||||
|
||||
TEST(SphericalSingle, Addition) {
|
||||
SphericalSingle v1 =
|
||||
SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0));
|
||||
SphericalSingle v2 = SphericalSingle ::zero;
|
||||
SphericalSingle r = SphericalSingle ::zero;
|
||||
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
|
||||
|
||||
r = v1;
|
||||
r += v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
|
||||
|
||||
v2 = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0));
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
|
||||
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
|
||||
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
|
||||
|
||||
v2 = SphericalSingle(1, AngleSingle::Degrees(0), AngleSingle::Degrees(90));
|
||||
r = v1 + v2;
|
||||
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
|
||||
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
|
||||
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
|
||||
}
|
||||
|
||||
TEST(SphericalSingle, AdditionPerformance) {
|
||||
const int numIterations = 1000000; // Number of additions to test
|
||||
std::vector<SphericalSingle> sphericalObjects;
|
||||
|
||||
// Populate the vector with random SphericalOf objects
|
||||
for (int i = 0; i < numIterations; ++i) {
|
||||
float distance = (float)(rand() % 100);
|
||||
float horizontal = (float)(rand() % 180);
|
||||
float vertical = (float)(rand() % 360);
|
||||
SphericalSingle s = SphericalSingle::Deg(distance, horizontal, vertical);
|
||||
sphericalObjects.push_back(s);
|
||||
}
|
||||
|
||||
// Measure the time to perform multiple additions
|
||||
auto start = std::chrono::high_resolution_clock::now();
|
||||
|
||||
SphericalSingle result = SphericalSingle::zero; // Start with a
|
||||
// zero-initialized object
|
||||
|
||||
for (int i = 0; i < numIterations - 1; ++i) {
|
||||
result = result + sphericalObjects[i]; // Add objects
|
||||
// together
|
||||
}
|
||||
|
||||
auto end = std::chrono::high_resolution_clock::now();
|
||||
std::chrono::duration<double> duration = end - start;
|
||||
std::cout << "Time to perform " << numIterations - 1
|
||||
<< " additions: " << duration.count() << " seconds." << std::endl;
|
||||
|
||||
// Assert that the time taken is less than
|
||||
// 1 second (or any other performance
|
||||
// requirement)
|
||||
ASSERT_LE(duration.count(), 1.0) << "Performance test failed: "
|
||||
"Additions took longer than 1 "
|
||||
"second.";
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,131 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <math.h>
|
||||
#include <limits>
|
||||
|
||||
#include "SwingTwist.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(SwingTwistSingle, Quaternion) {
|
||||
Quaternion q;
|
||||
SwingTwistSingle s;
|
||||
Quaternion rq;
|
||||
|
||||
q = Quaternion::identity;
|
||||
s = SwingTwistSingle::FromQuaternion(q);
|
||||
rq = s.ToQuaternion();
|
||||
EXPECT_EQ(q, rq) << " 0 0 0 1 <-> SwingTwist";
|
||||
|
||||
q = Quaternion::Euler(90, 0, 0);
|
||||
s = SwingTwistSingle::FromQuaternion(q);
|
||||
rq = s.ToQuaternion();
|
||||
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 90 0 0 <-> SwingTwist";
|
||||
|
||||
q = Quaternion::Euler(0, 90, 0);
|
||||
s = SwingTwistSingle::FromQuaternion(q);
|
||||
rq = s.ToQuaternion();
|
||||
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
||||
|
||||
q = Quaternion::Euler(0, 0, 90);
|
||||
s = SwingTwistSingle::FromQuaternion(q);
|
||||
rq = s.ToQuaternion();
|
||||
EXPECT_EQ(q, rq) << " Euler 0 0 90 <-> SwingTwist";
|
||||
|
||||
q = Quaternion::Euler(0, 180, 0); // ==> spherical S(180 0)T0
|
||||
s = SwingTwistSingle::FromQuaternion(q);
|
||||
rq = s.ToQuaternion();
|
||||
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
||||
|
||||
q = Quaternion::Euler(0, 135, 0); // ==> spherical S(180 45)T0
|
||||
s = SwingTwistSingle::FromQuaternion(q);
|
||||
rq = s.ToQuaternion();
|
||||
EXPECT_LT(Quaternion::Angle(q, rq), 10e-2) << " Euler 0 90 0 <-> SwingTwist";
|
||||
}
|
||||
|
||||
TEST(SwingTwistSingle, AngleAxis) {
|
||||
SwingTwistSingle s;
|
||||
SwingTwistSingle r;
|
||||
|
||||
s = SwingTwistSingle::AngleAxis(0, DirectionSingle::up);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 up";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::up);
|
||||
s = SwingTwistSingle::Degrees(90, 0, 0);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "90 up";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::up);
|
||||
s = SwingTwistSingle::Degrees(180, 0, 0);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "180 up";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::up);
|
||||
s = SwingTwistSingle::Degrees(-90, 0, 0);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "270 up";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::right);
|
||||
s = SwingTwistSingle::Degrees(0, 90, 0);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "90 right";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::right);
|
||||
s = SwingTwistSingle::Degrees(0, 180, 0);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "180 right";
|
||||
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::right);
|
||||
s = SwingTwistSingle::Degrees(0, -90, 0);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "270 right";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(90, DirectionSingle::forward);
|
||||
s = SwingTwistSingle::Degrees(0, 0, 90);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "90 up";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(180, DirectionSingle::forward);
|
||||
s = SwingTwistSingle::Degrees(0, 0, 180);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "180 up";
|
||||
|
||||
r = SwingTwistSingle::AngleAxis(270, DirectionSingle::forward);
|
||||
s = SwingTwistSingle::Degrees(0, 0, -90);
|
||||
EXPECT_LT(SwingTwistSingle::Angle(r, s), AngleSingle::Degrees(10e-2f))
|
||||
<< "270 up";
|
||||
|
||||
auto r16 = SwingTwist16::AngleAxis(13, Direction16::down);
|
||||
auto s16 = SwingTwist16::Degrees(-13, 0, 0);
|
||||
EXPECT_LT(SwingTwist16::Angle(r16, s16), Angle16::Degrees(10e-2f))
|
||||
<< "270 up";
|
||||
}
|
||||
|
||||
TEST(SwingTwistSingle, Normalize) {
|
||||
SwingTwistSingle s;
|
||||
|
||||
s = SwingTwistSingle::Degrees(0, 0, 0);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(0, 0, 0)) << "0 0 0 Normalized";
|
||||
|
||||
s = SwingTwistSingle::Degrees(0, 180, 0);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 180)) << "0 180 0 Normalized";
|
||||
|
||||
s = SwingTwistSingle::Degrees(0, 180, 180);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(180, 0, 0)) << "0 180 180 Normalized";
|
||||
|
||||
s = SwingTwistSingle::Degrees(270, 90, 0);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 90, 0)) << "270 90 0 Normalized";
|
||||
|
||||
s = SwingTwistSingle::Degrees(270, 270, 0);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, -90, 0))
|
||||
<< "270 270 0 Normalized";
|
||||
|
||||
s = SwingTwistSingle::Degrees(270, 225, 0);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(90, -45, -180))
|
||||
<< "270 225 0 Normalized";
|
||||
|
||||
s = SwingTwistSingle::Degrees(270, 0, 225);
|
||||
EXPECT_EQ(s, SwingTwistSingle::Degrees(-90, 0, -135))
|
||||
<< "270 0 225 Normalized";
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,499 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "Vector2.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Vector2, FromPolar) {
|
||||
Vector2 v;
|
||||
PolarSingle p;
|
||||
Vector2 r;
|
||||
|
||||
v = Vector2(0, 1);
|
||||
p = PolarSingle::FromVector2(v);
|
||||
r = Vector2(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(r.x, 0.0F) << "FromPolar(0 1)";
|
||||
EXPECT_FLOAT_EQ(r.y, 1.0F) << "FromPolar(0 1)";
|
||||
|
||||
v = Vector2(1, 0);
|
||||
p = PolarSingle::FromVector2(v);
|
||||
r = Vector2(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(r.x, 1.0F) << "FromPolar(1 0)";
|
||||
EXPECT_NEAR(r.y, 0.0F, 1.0e-07) << "FromPolar(1 0)";
|
||||
|
||||
v = Vector2(0, 0);
|
||||
p = PolarSingle::FromVector2(v);
|
||||
r = Vector2(p);
|
||||
|
||||
EXPECT_FLOAT_EQ(r.x, 0.0F) << "FromPolar(0 0)";
|
||||
EXPECT_FLOAT_EQ(r.y, 0.0F) << "FromPolar(0 0)";
|
||||
}
|
||||
|
||||
TEST(Vector2, Magnitude) {
|
||||
Vector2 v = Vector2(1, 2);
|
||||
float m = 0;
|
||||
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude 1 2";
|
||||
|
||||
m = Vector2::Magnitude(v);
|
||||
EXPECT_FLOAT_EQ(m, 2.236068F) << "Vector2::Magnitude 1 2";
|
||||
|
||||
v = Vector2(-1, -2);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, 2.236068F) << "v.magnitude -1 -2";
|
||||
|
||||
v = Vector2(0, 0);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 ";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude INFINITY INFINITY ";
|
||||
|
||||
v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.magnitude -INFINITY -INFINITY ";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, SqrMagnitude) {
|
||||
Vector2 v = Vector2(1, 2);
|
||||
float m = 0;
|
||||
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, 5) << "v.sqrMagnitude 1 2";
|
||||
|
||||
m = Vector2::SqrMagnitude(v);
|
||||
EXPECT_FLOAT_EQ(m, 5) << "Vector2::SqrMagnitude 1 2";
|
||||
|
||||
v = Vector2(-1, -2);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, 5) << "v.sqrMagnitude -1 -2";
|
||||
|
||||
v = Vector2(0, 0);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, 0) << "v.sqrMagnitude 0 0 ";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.sqrMagnitude INFINITY INFINITY ";
|
||||
|
||||
v = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY) << "v.sqrMagnitude -INFINITY -INFINITY ";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Normalize) {
|
||||
bool r = false;
|
||||
|
||||
Vector2 v1 = Vector2(0, 2);
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = v1.normalized();
|
||||
EXPECT_TRUE(v == Vector2(0, 1)) << "v.normalized 0 2";
|
||||
|
||||
v = Vector2::Normalize(v1);
|
||||
EXPECT_TRUE(v == Vector2(0, 1)) << "Vector3::Normalize 0 2";
|
||||
|
||||
v1 = Vector2(0, -2);
|
||||
v = v1.normalized();
|
||||
EXPECT_TRUE(v == Vector2(0, -1)) << "v.normalized 0 -2";
|
||||
|
||||
v1 = Vector2(0, 0);
|
||||
v = v1.normalized();
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "v.normalized 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v1 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = v1.normalized();
|
||||
r = isnan(v.x) && isnan(v.y);
|
||||
EXPECT_TRUE(r) << "v.normalized INFINITY INFINITY";
|
||||
|
||||
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = v1.normalized();
|
||||
r = isnan(v.x) && isnan(v.y);
|
||||
EXPECT_TRUE(r) << "v.normalized -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Negate) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector2(-4, -5)) << "- 4 5";
|
||||
|
||||
v1 = Vector2(-4, -5);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector2(4, 5)) << "- -4 -5";
|
||||
|
||||
v1 = Vector2(0, 0);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "- 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v1 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "- INFINITY INFINITY";
|
||||
|
||||
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "- -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Subtract) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 - 1 2";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector2(5, 7)) << "4 5 - -1 -2";
|
||||
|
||||
v2 = Vector2(4, 5);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 - 4 5";
|
||||
v = v1;
|
||||
v -= v2;
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 - 4 5";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 - 0 0";
|
||||
|
||||
v -= v2;
|
||||
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 - 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 - INFINITY INFINITY";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 - -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Addition) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector2(5, 7)) << "4 5 + 1 2";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 + -1 -2";
|
||||
v = v1;
|
||||
v += v2;
|
||||
EXPECT_TRUE(v == Vector2(3, 3)) << "4 5 + -1 -2";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 + 0 0";
|
||||
v += v2;
|
||||
EXPECT_TRUE(v == Vector2(4, 5)) << "4 5 + 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 + INFINITY INFINITY";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 + -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Scale) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = Vector2::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector2(4, 10)) << "Scale 4 5 , 1 2";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
v = Vector2::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector2(-4, -10)) << "Scale 4 5 , -1 -2";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
v = Vector2::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "Scale 4 5 , 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = Vector2::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 + INFINITY INFINITY";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = Vector2::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 + -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Multiply) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
float f = 3;
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector2(12, 15)) << "4 5 * 3";
|
||||
|
||||
f = -3;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector2(-12, -15)) << "4 5 * -3";
|
||||
|
||||
f = 0;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 * 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
f = FLOAT_INFINITY;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 * INFINITY";
|
||||
|
||||
f = -FLOAT_INFINITY;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 * -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Divide) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
float f = 2;
|
||||
Vector2 v = Vector2::zero;
|
||||
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector2(2, 2.5F)) << "4 5 / 3";
|
||||
|
||||
f = -2;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector2(-2, -2.5F)) << "4 5 / -3";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
f = 0;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector2(FLOAT_INFINITY, FLOAT_INFINITY)) << "4 5 / 0";
|
||||
|
||||
f = FLOAT_INFINITY;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 / INFINITY";
|
||||
|
||||
f = -FLOAT_INFINITY;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector2(0, 0)) << "4 5 / -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Dot) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
float f = 0;
|
||||
|
||||
f = Vector2::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 14) << "Dot(4 5, 1 2)";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
f = Vector2::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, -14) << "Dot(4 5, -1 -2)";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
f = Vector2::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 0) << "Dot(4 5, 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector2::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Dot(4 5, INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector2::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, -FLOAT_INFINITY) << "Dot(4 5, -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Equality) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
bool r = false;
|
||||
|
||||
r = v1 == v2;
|
||||
EXPECT_FALSE(r) << "4 5 == 1 2";
|
||||
|
||||
v2 = Vector2(4, 5);
|
||||
r = v1 == v2;
|
||||
EXPECT_TRUE(r) << "4 5 == 1 2";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
r = v1 == v2;
|
||||
EXPECT_FALSE(r) << "4 5 == INFINITY INFINITY";
|
||||
|
||||
v1 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
r = v1 == v2;
|
||||
EXPECT_FALSE(r) << "-INFINITY -INFINITY == INFINITY INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Distance) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
float f = 0;
|
||||
|
||||
f = Vector2::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 4.24264F) << "Distance(4 5, 1 2)";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
f = Vector2::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 8.602325F) << "Distance(4 5, -1 -2)";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
f = Vector2::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 6.403124F) << "Distance(4 5, 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector2::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector2::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY) << "Distance(4 5, -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, Angle) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
float f = 0;
|
||||
bool r = false;
|
||||
|
||||
f = Vector2::Angle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 12.09476F) << "Angle(4 5, 1 2)";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
f = Vector2::Angle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 167.9052F) << "Angle(4 5, -1 -2)";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
f = Vector2::Angle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 0) << "Angle(4 5, 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector2::Angle(v1, v2);
|
||||
r = isnan(f);
|
||||
EXPECT_TRUE(r) << "Angle(4 5, INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector2::Angle(v1, v2);
|
||||
r = isnan(f);
|
||||
EXPECT_TRUE(r) << "Angle(4 5, -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector2, SignedAngle) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
float f = 0;
|
||||
bool r = false;
|
||||
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, -12.09476F) << "SignedAngle(4 5, 1 2)";
|
||||
|
||||
v2 = Vector2(-1, -2);
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 167.9052F) << "SignedAngle(4 5, -1 -2)";
|
||||
|
||||
v2 = Vector2(0, 0);
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 0) << "SignedAngle(4 5, 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector2(FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
r = isnan(f);
|
||||
EXPECT_TRUE(r) << "SignedAngle(4 5, INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector2(-FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
r = isnan(f);
|
||||
EXPECT_TRUE(r) << "SignedAngle(4 5, -INFINITY -INFINITY)";
|
||||
}
|
||||
|
||||
v1 = Vector2(0, 1);
|
||||
v2 = Vector2(1, 0);
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 90.0F) << "SignedAngle(0 1, 1 0)";
|
||||
|
||||
v1 = Vector2(0, 1);
|
||||
v2 = Vector2(0, -1);
|
||||
f = Vector2::SignedAngle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 180.0F) << "SignedAngle(0 1, 1 0)";
|
||||
}
|
||||
|
||||
TEST(Vector2, Rotate) {
|
||||
Vector2 v1 = Vector2(1, 2);
|
||||
Vector2 r = Vector2(0, 0);
|
||||
|
||||
r = Vector2::Rotate(v1, AngleSingle::Degrees(0));
|
||||
EXPECT_FLOAT_EQ(Vector2::Distance(r, v1), 0);
|
||||
|
||||
r = Vector2::Rotate(v1, AngleSingle::Degrees(180));
|
||||
EXPECT_NEAR(Vector2::Distance(r, Vector2(-1, -2)), 0, 1.0e-06);
|
||||
|
||||
r = Vector2::Rotate(v1, AngleSingle::Degrees(-90));
|
||||
EXPECT_NEAR(Vector2::Distance(r, Vector2(2, -1)), 0, 1.0e-06);
|
||||
|
||||
r = Vector2::Rotate(v1, AngleSingle::Degrees(270));
|
||||
EXPECT_NEAR(Vector2::Distance(r, Vector2(2, -1)), 0, 1.0e-06);
|
||||
}
|
||||
|
||||
TEST(Vector2, Lerp) {
|
||||
Vector2 v1 = Vector2(4, 5);
|
||||
Vector2 v2 = Vector2(1, 2);
|
||||
Vector2 r = Vector2(0, 0);
|
||||
|
||||
r = Vector2::Lerp(v1, v2, 0);
|
||||
EXPECT_FLOAT_EQ(Vector2::Distance(r, v1), 0);
|
||||
|
||||
r = Vector2::Lerp(v1, v2, 1);
|
||||
EXPECT_FLOAT_EQ(Vector2::Distance(r, v2), 0);
|
||||
|
||||
r = Vector2::Lerp(v1, v2, 0.5f);
|
||||
EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(2.5f, 3.5f)), 0);
|
||||
|
||||
r = Vector2::Lerp(v1, v2, -1);
|
||||
EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(7.0f, 8.0f)), 0);
|
||||
|
||||
r = Vector2::Lerp(v1, v2, 2);
|
||||
EXPECT_FLOAT_EQ(Vector2::Distance(r, Vector2(-2.0, -1.0f)), 0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,583 +0,0 @@
|
||||
#if GTEST
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <math.h>
|
||||
|
||||
#include "Vector3.h"
|
||||
|
||||
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
|
||||
|
||||
TEST(Vector3, FromSpherical) {
|
||||
Vector3 v = Vector3(0, 0, 1);
|
||||
SphericalOf<float> s = SphericalOf<float>::FromVector3(v);
|
||||
Vector3 r = Vector3(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 0 1";
|
||||
EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 0 0 1";
|
||||
EXPECT_FLOAT_EQ(r.Forward(), 1.0F) << "toVector3.z 0 0 1";
|
||||
|
||||
v = Vector3(0, 1, 0);
|
||||
s = SphericalOf<float>::FromVector3(v);
|
||||
r = Vector3(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 1 0";
|
||||
EXPECT_FLOAT_EQ(r.Up(), 1.0F) << "toVector3.y 0 1 0";
|
||||
EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 0 1 0";
|
||||
|
||||
v = Vector3(1, 0, 0);
|
||||
s = SphericalOf<float>::FromVector3(v);
|
||||
r = Vector3(s);
|
||||
|
||||
EXPECT_FLOAT_EQ(r.Right(), 1.0F) << "toVector3.x 1 0 0";
|
||||
EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 1 0 0";
|
||||
EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 1 0 0";
|
||||
}
|
||||
|
||||
TEST(Vector3, Magnitude) {
|
||||
Vector3 v = Vector3(1, 2, 3);
|
||||
float m = 0;
|
||||
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, 3.741657F) << "v.magnitude 1 2 3";
|
||||
|
||||
m = Vector3::Magnitude(v);
|
||||
EXPECT_FLOAT_EQ(m, 3.741657F) << "Vector3::Magnitude 1 2 3";
|
||||
|
||||
v = Vector3(-1, -2, -3);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, 3.741657F) << "v.magnitude -1 -2 -3";
|
||||
|
||||
v = Vector3(0, 0, 0);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 0 ";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
|
||||
<< "v.magnitude INFINITY INFINITY INFINITY ";
|
||||
|
||||
v = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
m = v.magnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
|
||||
<< "v.magnitude -INFINITY -INFINITY -INFINITY ";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, SqrMagnitude) {
|
||||
Vector3 v = Vector3(1, 2, 3);
|
||||
float m = 0;
|
||||
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, 14) << "v.sqrMagnitude 1 2 3";
|
||||
|
||||
m = Vector3::SqrMagnitude(v);
|
||||
EXPECT_FLOAT_EQ(m, 14) << "Vector3::SqrMagnitude 1 2 3";
|
||||
|
||||
v = Vector3(-1, -2, -3);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, 14) << "v.sqrMagnitude -1 -2 -3";
|
||||
|
||||
v = Vector3(0, 0, 0);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, 0) << "v.sqrMagnitude 0 0 0 ";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
|
||||
<< "v.sqrMagnitude INFINITY INFINITY INFINITY ";
|
||||
|
||||
v = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
m = v.sqrMagnitude();
|
||||
EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
|
||||
<< "v.sqrMagnitude -INFINITY -INFINITY -INFINITY ";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Normalize) {
|
||||
bool r = false;
|
||||
|
||||
Vector3 v1 = Vector3(0, 2, 0);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = v1.normalized();
|
||||
EXPECT_TRUE(v == Vector3(0, 1, 0)) << "v.normalized 0 2 0";
|
||||
|
||||
v = Vector3::Normalize(v1);
|
||||
EXPECT_TRUE(v == Vector3(0, 1, 0)) << "Vector3::Normalize 0 2 0";
|
||||
|
||||
v1 = Vector3(0, -2, 0);
|
||||
v = v1.normalized();
|
||||
EXPECT_TRUE(v == Vector3(0, -1, 0)) << "v.normalized 0 -2 0";
|
||||
|
||||
v1 = Vector3(0, 0, 0);
|
||||
v = v1.normalized();
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "v.normalized 0 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = v1.normalized();
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "v.normalized INFINITY INFINITY INFINITY";
|
||||
|
||||
v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = v1.normalized();
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "v.normalized -INFINITY -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Negate) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector3(-4, -5, -6)) << "- 4 5 6";
|
||||
|
||||
v1 = Vector3(-4, -5, -6);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector3(4, 5, 6)) << "- -4 -5 -6";
|
||||
|
||||
v1 = Vector3(0, 0, 0);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "- 0 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "- INFINITY INFINITY INFINITY";
|
||||
|
||||
v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = -v1;
|
||||
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "- -INFINITY -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Subtract) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector3(3, 3, 3)) << "4 5 6 - 1 2 3";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 - -1 -2 -3";
|
||||
|
||||
v2 = Vector3(4, 5, 6);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 - 4 5 6";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector3(4, 5, 6)) << "4 5 6 - 0 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 6 - INFINITY INFINITY INFINITY";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = v1 - v2;
|
||||
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 6 - -INFINITY -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Addition) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 + 1 2 3";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector3(3, 3, 3)) << "4 5 6 + -1 -2 -3";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector3(4, 5, 6)) << "4 5 6 + 0 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 6 + INFINITY INFINITY INFINITY";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = v1 + v2;
|
||||
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 6 + -INFINITY -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Scale) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = Vector3::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector3(4, 10, 18)) << "Scale 4 5 6 , 1 2 3";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
v = Vector3::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector3(-4, -10, -18)) << "Scale 4 5 6 , -1 -2 -3";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
v = Vector3::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "Scale 4 5 6 , 0 0 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = Vector3::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 6 + INFINITY INFINITY INFINITY";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = Vector3::Scale(v1, v2);
|
||||
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 6 + -INFINITY -INFINITY -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Multiply) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
float f = 3;
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector3(12, 15, 18)) << "4 5 6 * 3";
|
||||
|
||||
f = -3;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector3(-12, -15, -18)) << "4 5 6 * -3";
|
||||
|
||||
f = 0;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 * 0";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
f = FLOAT_INFINITY;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 6 * INFINITY";
|
||||
|
||||
f = -FLOAT_INFINITY;
|
||||
v = v1 * f;
|
||||
EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
|
||||
<< "4 5 6 * -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Divide) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
float f = 2;
|
||||
Vector3 v = Vector3::zero;
|
||||
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector3(2, 2.5F, 3)) << "4 5 6 / 3";
|
||||
|
||||
f = -2;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector3(-2, -2.5F, -3)) << "4 5 6 / -3";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
f = 0;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
|
||||
<< "4 5 6 / 0";
|
||||
|
||||
f = FLOAT_INFINITY;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 / INFINITY";
|
||||
|
||||
f = -FLOAT_INFINITY;
|
||||
v = v1 / f;
|
||||
EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 / -INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Dot) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
float f = 0;
|
||||
|
||||
f = Vector3::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 32) << "Dot(4 5 6, 1 2 3)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
f = Vector3::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, -32) << "Dot(4 5 6, -1 -2 -3)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
f = Vector3::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 0) << "Dot(4 5 6, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector3::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
|
||||
<< "Dot(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector3::Dot(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, -FLOAT_INFINITY)
|
||||
<< "Dot(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Equality) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
bool r = false;
|
||||
|
||||
r = v1 == v2;
|
||||
EXPECT_FALSE(r) << "4 5 6 == 1 2 3";
|
||||
|
||||
v2 = Vector3(4, 5, 6);
|
||||
r = v1 == v2;
|
||||
EXPECT_TRUE(r) << "4 5 6 == 1 2 3";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
r = v1 == v2;
|
||||
EXPECT_FALSE(r) << "4 5 6 == INFINITY INFINITY INFINITY";
|
||||
|
||||
v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
r = v1 == v2;
|
||||
EXPECT_FALSE(r)
|
||||
<< "-INFINITY -INFINITY -INFINITY == INFINITY INFINITY INFINITY";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Distance) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
float f = 0;
|
||||
|
||||
f = Vector3::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 5.19615221F) << "Distance(4 5 6, 1 2 3)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
f = Vector3::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 12.4498997F) << "Distance(4 5 6, -1 -2 -3)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
f = Vector3::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, 8.77496433F) << "Distance(4 5 6, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector3::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
|
||||
<< "Distance(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector3::Distance(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
|
||||
<< "Distance(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Cross) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v = Vector3::zero;
|
||||
bool r = false;
|
||||
|
||||
v = Vector3::Cross(v1, v2);
|
||||
r = v == Vector3(3, -6, 3);
|
||||
EXPECT_TRUE(r) << "Cross(4 5 6, 1 2 3)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
v = Vector3::Cross(v1, v2);
|
||||
r = v == Vector3(-3, 6, -3);
|
||||
EXPECT_TRUE(r) << "Cross(4 5 6, -1 -2 -3)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
v = Vector3::Cross(v1, v2);
|
||||
r = v == Vector3(0, 0, 0);
|
||||
EXPECT_TRUE(r) << "Cross(4 5 6, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = Vector3::Cross(v1, v2);
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "Cross(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = Vector3::Cross(v1, v2);
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "Cross(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Project) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v = Vector3::zero;
|
||||
bool r = false;
|
||||
|
||||
v = Vector3::Project(v1, v2);
|
||||
r = v == Vector3(2.28571439F, 4.57142878F, 6.85714293F);
|
||||
EXPECT_TRUE(r) << "Project(4 5 6, 1 2 3)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
v = Vector3::Project(v1, v2);
|
||||
r = v == Vector3(2.28571439F, 4.57142878F, 6.85714293F);
|
||||
EXPECT_TRUE(r) << "Project(4 5 6, -1 -2 -3)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
v = Vector3::Project(v1, v2);
|
||||
r = v == Vector3(0, 0, 0);
|
||||
EXPECT_TRUE(r) << "Project(4 5 6, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = Vector3::Project(v1, v2);
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "Project(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = Vector3::Project(v1, v2);
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "Project(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, ProjectOnPlane) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v = Vector3::zero;
|
||||
bool r = false;
|
||||
|
||||
v = Vector3::ProjectOnPlane(v1, v2);
|
||||
r = v == Vector3(1.71428561F, 0.428571224F, -0.857142925F);
|
||||
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, 1 2 3)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
v = Vector3::ProjectOnPlane(v1, v2);
|
||||
r = v == Vector3(1.71428561F, 0.428571224F, -0.857142925F);
|
||||
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -1 -2 -3)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
v = Vector3::ProjectOnPlane(v1, v2);
|
||||
r = v == Vector3(4, 5, 6);
|
||||
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
v = Vector3::ProjectOnPlane(v1, v2);
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
v = Vector3::ProjectOnPlane(v1, v2);
|
||||
r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
|
||||
EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Angle) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
AngleOf<float> f = AngleOf<float>::Degrees(0);
|
||||
bool r = false;
|
||||
|
||||
f = Vector3::Angle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F) << "Angle(4 5 6, 1 2 3)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
f = Vector3::Angle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F) << "Angle(4 5 6, -1 -2 -3)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
f = Vector3::Angle(v1, v2);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "Angle(4 5 6, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector3::Angle(v1, v2);
|
||||
r = isnan(f.InDegrees());
|
||||
EXPECT_TRUE(r) << "Angle(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector3::Angle(v1, v2);
|
||||
r = isnan(f.InDegrees());
|
||||
EXPECT_TRUE(r) << "Angle(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, SignedAngle) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 v3 = Vector3(7, 8, -9);
|
||||
AngleOf<float> f = AngleOf<float>::Degrees(0);
|
||||
bool r = false;
|
||||
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), -12.9331388F)
|
||||
<< "SignedAngle(4 5 6, 1 2 3, 7 8 -9)";
|
||||
|
||||
v2 = Vector3(-1, -2, -3);
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F)
|
||||
<< "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)";
|
||||
|
||||
v2 = Vector3(0, 0, 0);
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )";
|
||||
|
||||
v2 = Vector3(1, 2, 3);
|
||||
|
||||
v3 = Vector3(-7, -8, 9);
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F)
|
||||
<< "SignedAngle(4 5 6, 1 2 3, -7 -8 9)";
|
||||
|
||||
v3 = Vector3(0, 0, 0);
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)";
|
||||
|
||||
if (std::numeric_limits<float>::is_iec559) {
|
||||
v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
r = isnan(f.InDegrees());
|
||||
EXPECT_TRUE(r) << "SignedAngle(4 5 6, INFINITY INFINITY INFINITY)";
|
||||
|
||||
v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
|
||||
f = Vector3::SignedAngle(v1, v2, v3);
|
||||
r = isnan(f.InDegrees());
|
||||
EXPECT_TRUE(r) << "SignedAngle(4 5 6, -INFINITY -INFINITY -INFINITY)";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Vector3, Lerp) {
|
||||
Vector3 v1 = Vector3(4, 5, 6);
|
||||
Vector3 v2 = Vector3(1, 2, 3);
|
||||
Vector3 r = Vector3(0, 0, 0);
|
||||
|
||||
r = Vector3::Lerp(v1, v2, 0);
|
||||
EXPECT_FLOAT_EQ(Vector3::Distance(r, v1), 0);
|
||||
|
||||
r = Vector3::Lerp(v1, v2, 1);
|
||||
EXPECT_FLOAT_EQ(Vector3::Distance(r, v2), 0);
|
||||
|
||||
r = Vector3::Lerp(v1, v2, 0.5f);
|
||||
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(2.5f, 3.5f, 4.5f)), 0);
|
||||
|
||||
r = Vector3::Lerp(v1, v2, -1);
|
||||
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(7.0f, 8.0f, 9.0f)), 0);
|
||||
|
||||
r = Vector3::Lerp(v1, v2, 2);
|
||||
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(-2.0, -1.0f, 0.0f)), 0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,366 +0,0 @@
|
||||
#include "LocalParticipant.h"
|
||||
|
||||
#include "Thing.h"
|
||||
|
||||
#include "Arduino/ArduinoParticipant.h"
|
||||
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
#include <winsock2.h>
|
||||
#include <ws2tcpip.h>
|
||||
#include "Windows/WindowsParticipant.h"
|
||||
#pragma comment(lib, "ws2_32.lib")
|
||||
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
#include <arpa/inet.h>
|
||||
#include <fcntl.h> // For fcntl
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
#include <chrono>
|
||||
#include "Posix/PosixParticipant.h"
|
||||
#endif
|
||||
|
||||
#include <string.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
// LocalParticipant::LocalParticipant() {}
|
||||
|
||||
LocalParticipant::LocalParticipant(int port) {
|
||||
this->ipAddress = "0.0.0.0";
|
||||
this->port = port;
|
||||
if (this->port == 0)
|
||||
this->isIsolated = true;
|
||||
}
|
||||
|
||||
LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
|
||||
this->ipAddress = "0.0.0.0"; // ipAddress; // maybe this is not needed
|
||||
// anymore, keeping it to "0.0.0.0"
|
||||
this->port = port;
|
||||
if (this->port == 0)
|
||||
this->isIsolated = true;
|
||||
else
|
||||
this->remoteSite = new Participant(ipAddress, port);
|
||||
}
|
||||
|
||||
static LocalParticipant* isolatedParticipant = nullptr;
|
||||
|
||||
LocalParticipant* LocalParticipant::Isolated() {
|
||||
if (isolatedParticipant == nullptr)
|
||||
isolatedParticipant = new LocalParticipant(0);
|
||||
return isolatedParticipant;
|
||||
}
|
||||
|
||||
void LocalParticipant::begin() {
|
||||
if (this->isIsolated)
|
||||
return;
|
||||
|
||||
SetupUDP(this->port, this->ipAddress, this->port);
|
||||
}
|
||||
|
||||
void LocalParticipant::SetupUDP(int localPort,
|
||||
const char* remoteIpAddress,
|
||||
int remotePort) {
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
Windows::LocalParticipant* thisWindows =
|
||||
static_cast<Windows::LocalParticipant*>(this);
|
||||
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
Posix::LocalParticipant* thisPosix =
|
||||
static_cast<Posix::LocalParticipant*>(this);
|
||||
thisPosix->Setup(localPort, remoteIpAddress, remotePort);
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
thisArduino->Setup(localPort, remoteIpAddress, remotePort);
|
||||
#endif
|
||||
this->connected = true;
|
||||
}
|
||||
|
||||
void LocalParticipant::Update(unsigned long currentTimeMs) {
|
||||
if (currentTimeMs == 0) {
|
||||
currentTimeMs = Thing::GetTimeMs();
|
||||
// #if defined(ARDUINO)
|
||||
// currentTimeMs = millis();
|
||||
// #elif defined(__unix__) || defined(__APPLE__)
|
||||
// auto now = std::chrono::steady_clock::now();
|
||||
// auto ms =
|
||||
// std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
|
||||
// currentTimeMs = static_cast<unsigned long>(ms.count());
|
||||
// #endif
|
||||
}
|
||||
|
||||
if (this->isIsolated == false) {
|
||||
if (this->connected == false)
|
||||
begin();
|
||||
|
||||
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
|
||||
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
|
||||
if (this->remoteSite == nullptr)
|
||||
this->Publish(msg);
|
||||
else
|
||||
this->Send(this->remoteSite, msg);
|
||||
delete msg;
|
||||
|
||||
this->nextPublishMe = currentTimeMs + this->publishInterval;
|
||||
}
|
||||
this->ReceiveUDP();
|
||||
}
|
||||
|
||||
for (Thing* thing : this->things) {
|
||||
if (thing != nullptr) {
|
||||
thing->Update(currentTimeMs);
|
||||
if (this->isIsolated == false) {
|
||||
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
||||
for (Participant* sender : this->senders)
|
||||
this->Send(sender, poseMsg);
|
||||
delete poseMsg;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LocalParticipant::ReceiveUDP() {
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
Windows::LocalParticipant* thisWindows =
|
||||
static_cast<Windows::LocalParticipant*>(this);
|
||||
thisWindows->Receive();
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
Posix::LocalParticipant* thisPosix =
|
||||
static_cast<Posix::LocalParticipant*>(this);
|
||||
thisPosix->Receive();
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
thisArduino->Receive();
|
||||
#endif
|
||||
}
|
||||
|
||||
Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) {
|
||||
for (Participant* sender : this->senders) {
|
||||
if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
|
||||
return sender;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) {
|
||||
// std::cout << "New Participant " << ipAddress << ":" << port << "\n";
|
||||
Participant* participant = new Participant(ipAddress, port);
|
||||
#if defined(NO_STD)
|
||||
participant->networkId = this->senderCount;
|
||||
this->senders[this->senderCount++] = participant;
|
||||
#else
|
||||
participant->networkId = (unsigned char)this->senders.size();
|
||||
this->senders.push_back(participant);
|
||||
#endif
|
||||
return participant;
|
||||
}
|
||||
|
||||
#pragma region Send
|
||||
|
||||
void LocalParticipant::SendThingInfo(Participant* remoteParticipant,
|
||||
Thing* thing) {
|
||||
// std::cout << "Send thing info " << (int)thing->id << " \n";
|
||||
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||
this->Send(remoteParticipant, thingMsg);
|
||||
delete thingMsg;
|
||||
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||
this->Send(remoteParticipant, nameMsg);
|
||||
delete nameMsg;
|
||||
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
|
||||
this->Send(remoteParticipant, modelMsg);
|
||||
delete modelMsg;
|
||||
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
|
||||
this->Send(remoteParticipant, poseMsg);
|
||||
delete poseMsg;
|
||||
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
|
||||
this->Send(remoteParticipant, customMsg);
|
||||
delete customMsg;
|
||||
}
|
||||
|
||||
bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
|
||||
int bufferSize = msg->Serialize(this->buffer);
|
||||
if (bufferSize <= 0)
|
||||
return true;
|
||||
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
Windows::LocalParticipant* thisWindows =
|
||||
static_cast<Windows::LocalParticipant*>(this);
|
||||
return thisWindows->Send(remoteParticipant, bufferSize);
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
Posix::LocalParticipant* thisPosix =
|
||||
static_cast<Posix::LocalParticipant*>(this);
|
||||
return thisPosix->Send(remoteParticipant, bufferSize);
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
return thisArduino->Send(remoteParticipant, bufferSize);
|
||||
#endif
|
||||
}
|
||||
|
||||
void LocalParticipant::PublishThingInfo(Thing* thing) {
|
||||
// std::cout << "Publish thing info" << thing->networkId << "\n";
|
||||
// Strange, when publishing, the network id is irrelevant, because it is
|
||||
// connected to a specific site...
|
||||
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||
this->Publish(thingMsg);
|
||||
delete thingMsg;
|
||||
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||
this->Publish(nameMsg);
|
||||
delete nameMsg;
|
||||
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
|
||||
this->Publish(modelMsg);
|
||||
delete modelMsg;
|
||||
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
|
||||
this->Publish(poseMsg);
|
||||
delete poseMsg;
|
||||
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
|
||||
this->Publish(customMsg);
|
||||
delete customMsg;
|
||||
}
|
||||
|
||||
bool LocalParticipant::Publish(IMessage* msg) {
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
Windows::LocalParticipant* thisWindows =
|
||||
static_cast<Windows::LocalParticipant*>(this);
|
||||
return thisWindows->Publish(msg);
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
Posix::LocalParticipant* thisPosix =
|
||||
static_cast<Posix::LocalParticipant*>(this);
|
||||
return thisPosix->Publish(msg);
|
||||
#elif defined(ARDUINO)
|
||||
Arduino::LocalParticipant* thisArduino =
|
||||
static_cast<Arduino::LocalParticipant*>(this);
|
||||
return thisArduino->Publish(msg);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Send
|
||||
#pragma endregion
|
||||
|
||||
#pragma region Receive
|
||||
|
||||
void LocalParticipant::ReceiveData(unsigned char packetSize,
|
||||
char* senderIpAddress,
|
||||
unsigned int senderPort) {
|
||||
Participant* remoteParticipant =
|
||||
this->GetParticipant(senderIpAddress, senderPort);
|
||||
if (remoteParticipant == nullptr) {
|
||||
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
|
||||
// std::cout << "New sender " << sender_ipAddress << ":" << sender_port
|
||||
// << "\n";
|
||||
// std::cout << "New remote participant " << remoteParticipant->ipAddress
|
||||
// << ":" << remoteParticipant->port << " "
|
||||
// << (int)remoteParticipant->networkId << "\n";
|
||||
}
|
||||
|
||||
ReceiveData(packetSize, remoteParticipant);
|
||||
}
|
||||
|
||||
void LocalParticipant::ReceiveData(unsigned char bufferSize,
|
||||
Participant* remoteParticipant) {
|
||||
unsigned char msgId = this->buffer[0];
|
||||
// std::cout << "receive msg " << (int)msgId << "\n";
|
||||
switch (msgId) {
|
||||
case ParticipantMsg::id: {
|
||||
ParticipantMsg* msg = new ParticipantMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
case SiteMsg::id: {
|
||||
SiteMsg* msg = new SiteMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
case InvestigateMsg::id: {
|
||||
InvestigateMsg* msg = new InvestigateMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
case ThingMsg::id: {
|
||||
ThingMsg* msg = new ThingMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
case NameMsg::id: {
|
||||
NameMsg* msg = new NameMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
case PoseMsg::id: {
|
||||
PoseMsg* msg = new PoseMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
case BinaryMsg::id: {
|
||||
BinaryMsg* msg = new BinaryMsg(this->buffer);
|
||||
Process(remoteParticipant, msg);
|
||||
delete msg;
|
||||
} break;
|
||||
};
|
||||
}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, SiteMsg* msg) {
|
||||
// std::cout << this->name << ": process NetworkId [" << (int)this->networkId
|
||||
// << "/" << (int)msg->networkId << "]\n";
|
||||
if (this->networkId != msg->networkId) {
|
||||
this->networkId = msg->networkId;
|
||||
// std::cout << this->things.size() << " things\n";
|
||||
for (Thing* thing : this->things)
|
||||
this->SendThingInfo(sender, thing);
|
||||
}
|
||||
}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
|
||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
||||
if (thing != nullptr) {
|
||||
int nameLength = msg->nameLength;
|
||||
int stringLen = nameLength + 1;
|
||||
char* thingName = new char[stringLen];
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
strncpy_s(thingName, stringLen, msg->name,
|
||||
stringLen - 1); // Leave space for null terminator
|
||||
#else
|
||||
// Use strncpy with bounds checking for other platforms (Arduino, POSIX,
|
||||
// ESP-IDF)
|
||||
strncpy(thingName, msg->name,
|
||||
stringLen - 1); // Leave space for null terminator
|
||||
thingName[stringLen - 1] = '\0'; // Ensure null termination
|
||||
#endif
|
||||
thingName[nameLength] = '\0';
|
||||
thing->name = thingName;
|
||||
// std::cout << "thing name = " << thing->name << " length = " << nameLength
|
||||
// << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {}
|
||||
|
||||
void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
|
||||
// std::cout << this->name << ": process Binary [" << (int)this->networkId <<
|
||||
// "/"
|
||||
// << (int)msg->networkId << "]\n";
|
||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
||||
if (thing != nullptr)
|
||||
thing->ProcessBinary(msg->bytes);
|
||||
else {
|
||||
thing = this->Get(msg->networkId, msg->thingId);
|
||||
if (thing != nullptr)
|
||||
thing->ProcessBinary(msg->bytes);
|
||||
// else
|
||||
// std::cout << "custom msg for unknown thing [" << (int)msg->networkId
|
||||
// << "/" << (int)msg->thingId << "]\n";
|
||||
}
|
||||
}
|
||||
|
||||
// Receive
|
||||
#pragma endregion
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,140 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Messages/BinaryMsg.h"
|
||||
#include "Messages/InvestigateMsg.h"
|
||||
#include "Messages/ModelUrlMsg.h"
|
||||
#include "Messages/NameMsg.h"
|
||||
#include "Messages/ParticipantMsg.h"
|
||||
#include "Messages/PoseMsg.h"
|
||||
#include "Messages/SiteMsg.h"
|
||||
#include "Messages/ThingMsg.h"
|
||||
#include "Participant.h"
|
||||
|
||||
#if !defined(NO_STD)
|
||||
#include <list>
|
||||
#endif
|
||||
|
||||
#if defined(_WIN32) || defined(_WIN64)
|
||||
#include <winsock2.h>
|
||||
#elif defined(__unix__) || defined(__APPLE__)
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
#elif defined(ARDUINO)
|
||||
// #include <WiFiUdp.h>
|
||||
#endif
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
constexpr int MAX_SENDER_COUNT = 256;
|
||||
|
||||
/// @brief A local participant is the local device which can communicate with
|
||||
/// other participants It manages all local things and communcation with other
|
||||
/// participants. Each application has a local participant which is usually
|
||||
/// explicit in the code. An participant can be isolated. In that case it is
|
||||
/// standalong and does not communicate with other participants.
|
||||
///
|
||||
/// It is possible to work with an hidden participant by creating things without
|
||||
/// specifying a participant in the constructor. In that case an hidden isolated
|
||||
/// participant is created which can be obtained using
|
||||
/// RoboidControl::LocalParticipant::Isolated().
|
||||
/// @sa RoboidControl::Thing::Thing()
|
||||
class LocalParticipant : public Participant {
|
||||
public:
|
||||
/// @brief Create a participant without connecting to a site
|
||||
/// @param port The port on which the participant communicates
|
||||
/// These participant typically broadcast Participant messages to let site
|
||||
/// servers on the local network know their presence. Alternatively they can
|
||||
/// broadcast information which can be used directly by other participants.
|
||||
LocalParticipant(int port = 7681);
|
||||
/// @brief Create a participant which will try to connect to a site.
|
||||
/// @param ipAddress The IP address of the site
|
||||
/// @param port The port used by the site
|
||||
LocalParticipant(const char* ipAddress, int port = 7681);
|
||||
// Note to self: one cannot specify the port used by the local participant
|
||||
// now!!
|
||||
|
||||
/// @brief Isolated participant is used when the application is run without
|
||||
/// networking
|
||||
/// @return A participant without networking support
|
||||
static LocalParticipant* Isolated();
|
||||
|
||||
/// @brief True if the participant is running isolated.
|
||||
/// Isolated participants do not communicate with other participants
|
||||
bool isIsolated = false;
|
||||
|
||||
/// The interval in milliseconds for publishing (broadcasting) data on the
|
||||
/// local network
|
||||
long publishInterval = 3000; // 3 seconds
|
||||
|
||||
/// @brief The name of the participant
|
||||
const char* name = "LocalParticipant";
|
||||
|
||||
// int localPort = 0;
|
||||
|
||||
/// @brief The remote site when this participant is connected to a site
|
||||
Participant* remoteSite = nullptr;
|
||||
|
||||
#if defined(ARDUINO)
|
||||
// const char* remoteIpAddress = nullptr;
|
||||
// unsigned short remotePort = 0;
|
||||
// char* broadcastIpAddress = nullptr;
|
||||
|
||||
// WiFiUDP udp;
|
||||
#else
|
||||
|
||||
#if defined(__unix__) || defined(__APPLE__)
|
||||
int sock;
|
||||
#endif
|
||||
sockaddr_in remote_addr;
|
||||
sockaddr_in server_addr;
|
||||
sockaddr_in broadcast_addr;
|
||||
|
||||
#endif
|
||||
|
||||
void begin();
|
||||
bool connected = false;
|
||||
|
||||
virtual void Update(unsigned long currentTimeMs = 0);
|
||||
|
||||
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
|
||||
void PublishThingInfo(Thing* thing);
|
||||
|
||||
bool Send(Participant* remoteParticipant, IMessage* msg);
|
||||
bool Publish(IMessage* msg);
|
||||
|
||||
void ReceiveData(unsigned char bufferSize,
|
||||
char* senderIpAddress,
|
||||
unsigned int senderPort);
|
||||
void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
|
||||
|
||||
#if defined(NO_STD)
|
||||
unsigned char senderCount = 0;
|
||||
Participant* senders[MAX_SENDER_COUNT];
|
||||
#else
|
||||
std::list<Participant*> senders;
|
||||
#endif
|
||||
|
||||
protected:
|
||||
unsigned long nextPublishMe = 0;
|
||||
|
||||
char buffer[1024];
|
||||
|
||||
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
|
||||
|
||||
Participant* GetParticipant(const char* ipAddress, int port);
|
||||
Participant* AddParticipant(const char* ipAddress, int port);
|
||||
|
||||
void ReceiveUDP();
|
||||
|
||||
virtual void Process(Participant* sender, ParticipantMsg* msg);
|
||||
virtual void Process(Participant* sender, SiteMsg* msg);
|
||||
virtual void Process(Participant* sender, InvestigateMsg* msg);
|
||||
virtual void Process(Participant* sender, ThingMsg* msg);
|
||||
virtual void Process(Participant* sender, NameMsg* msg);
|
||||
virtual void Process(Participant* sender, PoseMsg* msg);
|
||||
virtual void Process(Participant* sender, BinaryMsg* msg);
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
33
Magnetometer.h
Normal file
33
Magnetometer.h
Normal file
@ -0,0 +1,33 @@
|
||||
#pragma once
|
||||
|
||||
#include "Sensor.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A Sensor which can measure the magnetic field
|
||||
class Magnetometer : public Sensor {
|
||||
public:
|
||||
Magnetometer();
|
||||
|
||||
/// @brief Returns the direction of the magnetic field relative to the forward
|
||||
/// direction
|
||||
/// @return The direction, negative is to the left, positive is to the right
|
||||
/// @note The actual unit (degrees, radians, -1..1, ...) depends on the
|
||||
/// sensor.
|
||||
virtual float GetDirection();
|
||||
/// @brief Returns the inclination of the magnetic field relative to the
|
||||
/// horizontal plane
|
||||
/// @return The direction, negative is downward, positive is upward
|
||||
/// @note The actual unit (degrees, radias, -1..1, ...) depends on the sensor.
|
||||
virtual float GetInclination();
|
||||
|
||||
/// @brief Returns the strength of the magnetic field
|
||||
/// @return The strength. This values should always be positive
|
||||
/// @note The actual unit (tesla, 0..1, ...) depends on the sensor.
|
||||
virtual unsigned float GetMagnitude();
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
} // namespace Passer
|
||||
using namespace Passer::RoboidControl;
|
||||
@ -1,34 +0,0 @@
|
||||
#include "BinaryMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
BinaryMsg::BinaryMsg(char* buffer) {
|
||||
unsigned char ix = 1;
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->bytes = buffer + ix; // This is only valid because the code ensures the the msg
|
||||
// lifetime is shorter than the buffer lifetime...
|
||||
}
|
||||
|
||||
BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thing->id;
|
||||
this->thing = thing;
|
||||
}
|
||||
|
||||
BinaryMsg::~BinaryMsg() {}
|
||||
|
||||
unsigned char BinaryMsg::Serialize(char* buffer) {
|
||||
unsigned char ix = this->length;
|
||||
this->thing->GenerateBinary(buffer, &ix);
|
||||
if (ix <= this->length) // in this case, no data is actually sent
|
||||
return 0;
|
||||
|
||||
buffer[0] = this->id;
|
||||
buffer[1] = this->networkId;
|
||||
buffer[2] = this->thingId;
|
||||
return ix;
|
||||
}
|
||||
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,38 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message to send thing-specific data
|
||||
class BinaryMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0xB1;
|
||||
/// @brief The length of the message without the binary data itslef
|
||||
static const unsigned length = 3;
|
||||
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief The ID of the thing
|
||||
unsigned char thingId;
|
||||
/// @brief The thing for which the binary data is communicated
|
||||
Thing* thing;
|
||||
|
||||
/// @brief The binary data which is communicated
|
||||
char* bytes = nullptr;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId The network ID of the thing
|
||||
/// @param thing The thing for which binary data is sent
|
||||
BinaryMsg(unsigned char networkId, Thing* thing);
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
BinaryMsg(char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~BinaryMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,22 +0,0 @@
|
||||
#include "DestroyMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thing->id;
|
||||
}
|
||||
|
||||
DestroyMsg::DestroyMsg(char* buffer) {}
|
||||
|
||||
DestroyMsg::~DestroyMsg() {}
|
||||
|
||||
unsigned char DestroyMsg::Serialize(char *buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,30 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message notifiying that a Thing no longer exists
|
||||
class DestroyMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0x20;
|
||||
/// @brief The length of the message
|
||||
static const unsigned length = 3;
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief The ID of the thing
|
||||
unsigned char thingId;
|
||||
|
||||
/// @brief Create a message for sending
|
||||
/// @param networkId The network ID of the thing
|
||||
/// @param thing The ID of the thing
|
||||
DestroyMsg(unsigned char networkId, Thing* thing);
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
DestroyMsg(char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~DestroyMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,24 +0,0 @@
|
||||
#include "InvestigateMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
InvestigateMsg::InvestigateMsg(char* buffer) {
|
||||
unsigned ix = 1; // first byte is msgId
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
}
|
||||
InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thingId;
|
||||
}
|
||||
|
||||
InvestigateMsg::~InvestigateMsg() {}
|
||||
unsigned char InvestigateMsg::Serialize(char* buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,30 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message to request details for a Thing
|
||||
class InvestigateMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0x81;
|
||||
/// @brief The length of the message
|
||||
static const unsigned char length = 3;
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief the ID of the thing
|
||||
unsigned char thingId;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId The network ID for the thing
|
||||
/// @param thingId The ID of the thing
|
||||
InvestigateMsg(unsigned char networkId, unsigned char thingId);
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
InvestigateMsg(char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~InvestigateMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,99 +0,0 @@
|
||||
#include "LowLevelMessages.h"
|
||||
|
||||
// #include <iostream>
|
||||
#include "LinearAlgebra/float16.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
void LowLevelMessages::SendAngle8(char* buffer,
|
||||
unsigned char* ix,
|
||||
const float angle) {
|
||||
Angle8 packedAngle2 = Angle8::Degrees(angle);
|
||||
buffer[(*ix)++] = packedAngle2.GetBinary();
|
||||
}
|
||||
Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
|
||||
unsigned char* startIndex) {
|
||||
unsigned char binary = buffer[(*startIndex)++];
|
||||
|
||||
Angle8 angle = Angle8::Binary(binary);
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendFloat16(char* buffer,
|
||||
unsigned char* ix,
|
||||
float value) {
|
||||
float16 value16 = float16(value);
|
||||
short binary = value16.getBinary();
|
||||
|
||||
buffer[(*ix)++] = (binary >> 8) & 0xFF;
|
||||
buffer[(*ix)++] = binary & 0xFF;
|
||||
}
|
||||
float LowLevelMessages::ReceiveFloat16(const char* buffer,
|
||||
unsigned char* startIndex) {
|
||||
unsigned char ix = *startIndex;
|
||||
unsigned char msb = buffer[ix++];
|
||||
unsigned char lsb = buffer[ix++];
|
||||
unsigned short value = msb << 8 | lsb;
|
||||
float16 f = float16();
|
||||
f.setBinary(value);
|
||||
|
||||
*startIndex = ix;
|
||||
return (float)f.toFloat();
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendSpherical(char* buffer,
|
||||
unsigned char* ix,
|
||||
Spherical s) {
|
||||
SendFloat16(buffer, ix, s.distance);
|
||||
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
|
||||
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
|
||||
}
|
||||
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
|
||||
unsigned char* startIndex) {
|
||||
float distance = ReceiveFloat16(buffer, startIndex);
|
||||
|
||||
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle horizontal = Angle::Radians(horizontal8.InRadians());
|
||||
|
||||
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
|
||||
Angle vertical = Angle::Radians(vertical8.InRadians());
|
||||
|
||||
Spherical s = Spherical(distance, horizontal, vertical);
|
||||
return s;
|
||||
}
|
||||
|
||||
void LowLevelMessages::SendQuat32(char* buffer,
|
||||
unsigned char* ix,
|
||||
SwingTwist rotation) {
|
||||
Quaternion q = rotation.ToQuaternion();
|
||||
unsigned char qx = (char)(q.x * 127 + 128);
|
||||
unsigned char qy = (char)(q.y * 127 + 128);
|
||||
unsigned char qz = (char)(q.z * 127 + 128);
|
||||
unsigned char qw = (char)(q.w * 255);
|
||||
if (q.w < 0) {
|
||||
qx = -qx;
|
||||
qy = -qy;
|
||||
qz = -qz;
|
||||
qw = -qw;
|
||||
}
|
||||
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
|
||||
// << "\n";
|
||||
buffer[(*ix)++] = qx;
|
||||
buffer[(*ix)++] = qy;
|
||||
buffer[(*ix)++] = qz;
|
||||
buffer[(*ix)++] = qw;
|
||||
}
|
||||
|
||||
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
|
||||
unsigned char* ix) {
|
||||
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
|
||||
float qw = buffer[(*ix)++] / 255.0F;
|
||||
Quaternion q = Quaternion(qx, qy, qz, qw);
|
||||
SwingTwist s = SwingTwist::FromQuaternion(q);
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,22 +0,0 @@
|
||||
#include "LinearAlgebra/Spherical.h"
|
||||
#include "LinearAlgebra/SwingTwist.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
class LowLevelMessages {
|
||||
public:
|
||||
static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
|
||||
static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
|
||||
|
||||
static void SendFloat16(char* buffer, unsigned char* ix, float value);
|
||||
static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
|
||||
|
||||
static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
|
||||
static Spherical ReceiveSpherical(const char* buffer,
|
||||
unsigned char* startIndex);
|
||||
|
||||
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
|
||||
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,36 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
#include "LowLevelMessages.h"
|
||||
//#include "Participant.h"
|
||||
#include "string.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
#pragma region IMessage
|
||||
|
||||
IMessage::IMessage() {}
|
||||
|
||||
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
|
||||
|
||||
// IMessage::IMessage(char* buffer) {}
|
||||
|
||||
unsigned char IMessage::Serialize(char* buffer) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// bool IMessage::SendMsg(LocalParticipant *client, IMessage msg) {
|
||||
// // return SendMsg(client, client.buffer, );nameLength
|
||||
// return client->SendBuffer(msg.Serialize(client->buffer));
|
||||
// }
|
||||
|
||||
// bool IMessage::Publish(LocalParticipant *participant) {
|
||||
// return participant->PublishBuffer(Serialize(participant->buffer));
|
||||
// }
|
||||
// bool IMessage::SendTo(LocalParticipant *participant) {
|
||||
// return participant->SendBuffer(Serialize(participant->buffer));
|
||||
// }
|
||||
|
||||
// IMessage
|
||||
#pragma endregion
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,22 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "LinearAlgebra/Spherical.h"
|
||||
#include "LinearAlgebra/SwingTwist.h"
|
||||
#include "Thing.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
class LocalParticipant;
|
||||
|
||||
class IMessage {
|
||||
public:
|
||||
IMessage();
|
||||
virtual unsigned char Serialize(char* buffer);
|
||||
|
||||
static unsigned char* ReceiveMsg(unsigned char packetSize);
|
||||
|
||||
// bool Publish(LocalParticipant *participant);
|
||||
// bool SendTo(LocalParticipant *participant);
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,68 +0,0 @@
|
||||
#include "ModelUrlMsg.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
// ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
|
||||
// unsigned char urlLength, const char *url,
|
||||
// float scale) {
|
||||
// this->networkId = networkId;
|
||||
// this->thingId = thingId;
|
||||
// this->urlLength = urlLength;
|
||||
// this->url = url;
|
||||
// this->scale = scale;
|
||||
// }
|
||||
|
||||
ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thing->id;
|
||||
if (thing->modelUrl == nullptr)
|
||||
this->urlLength = 0;
|
||||
else
|
||||
this->urlLength = (unsigned char)strlen(thing->modelUrl);
|
||||
|
||||
//this->url = thing->modelUrl; // dangerous!
|
||||
|
||||
// the url string in the buffer is not \0 terminated!
|
||||
char* url = new char[this->urlLength + 1];
|
||||
for (int i = 0; i < this->urlLength; i++)
|
||||
url[i] = thing->modelUrl[i];
|
||||
url[this->urlLength] = '\0';
|
||||
this->url = url;}
|
||||
|
||||
ModelUrlMsg::ModelUrlMsg(const char* buffer) {
|
||||
unsigned char ix = 1; // first byte is msg id
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->urlLength = buffer[ix++];
|
||||
// this->url = &buffer[ix]; // dangerous! name should not be used anymore after
|
||||
// // buffer has been re-used...
|
||||
|
||||
// the url string in the buffer is not \0 terminated!
|
||||
char* url = new char[this->urlLength + 1];
|
||||
for (int i = 0; i < this->urlLength; i++)
|
||||
url[i] = buffer[ix++];
|
||||
url[this->urlLength] = '\0';
|
||||
this->url = url;
|
||||
}
|
||||
|
||||
ModelUrlMsg::~ModelUrlMsg() {
|
||||
delete[] this->url;
|
||||
}
|
||||
|
||||
unsigned char ModelUrlMsg::Serialize(char* buffer) {
|
||||
if (this->urlLength == 0 || this->url == nullptr)
|
||||
return 0;
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
// LowLevelMessages::SendFloat16(buffer, &ix, this->scale);
|
||||
buffer[ix++] = this->urlLength;
|
||||
for (int urlIx = 0; urlIx < this->urlLength; urlIx++)
|
||||
buffer[ix++] = url[urlIx];
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,39 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message for communicating the URL for a model of the thing
|
||||
class ModelUrlMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0x90;
|
||||
/// @brief The length of the message without the URL string itself
|
||||
static const unsigned char length = 3;
|
||||
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief The ID of the thing
|
||||
unsigned char thingId;
|
||||
|
||||
/// @brief The length of the url st5ring, excluding the null terminator
|
||||
unsigned char urlLength;
|
||||
/// @brief The url of the model, not terminated by a null character
|
||||
const char* url;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId The network ID of the thing
|
||||
/// @param thing The thing for which to send the mode URL
|
||||
ModelUrlMsg(unsigned char networkId, Thing* thing);
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
ModelUrlMsg(const char* buffer);
|
||||
// ModelUrlMsg(unsigned char networkId, unsigned char thingId,
|
||||
// unsigned char urlLegth, const char *url, float scale = 1);
|
||||
|
||||
/// @brief Destructor for the message
|
||||
virtual ~ModelUrlMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,55 +0,0 @@
|
||||
#include "NameMsg.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thing->id;
|
||||
if (thing->name == nullptr)
|
||||
this->nameLength = 0;
|
||||
else
|
||||
this->nameLength = (unsigned char)strlen(thing->name);
|
||||
|
||||
// the name string in the buffer is not \0 terminated!
|
||||
char* name = new char[this->nameLength + 1];
|
||||
for (int i = 0; i < this->nameLength; i++)
|
||||
name[i] = thing->name[i];
|
||||
name[this->nameLength] = '\0';
|
||||
this->name = name;
|
||||
}
|
||||
|
||||
NameMsg::NameMsg(const char* buffer) {
|
||||
unsigned char ix = 1; // first byte is msg id
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->nameLength = buffer[ix++];
|
||||
// the name string in the buffer is not \0 terminated!
|
||||
char* name = new char[this->nameLength + 1];
|
||||
for (int i = 0; i < this->nameLength; i++)
|
||||
name[i] = buffer[ix++];
|
||||
name[this->nameLength] = '\0';
|
||||
this->name = name;
|
||||
}
|
||||
|
||||
NameMsg::~NameMsg() {
|
||||
delete[] this->name;
|
||||
}
|
||||
|
||||
unsigned char NameMsg::Serialize(char* buffer) {
|
||||
if (this->nameLength == 0 || this->name == nullptr)
|
||||
return 0;
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
buffer[ix++] = this->nameLength;
|
||||
for (int nameIx = 0; nameIx < this->nameLength; nameIx++)
|
||||
buffer[ix++] = this->name[nameIx];
|
||||
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,37 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message for communicating the name of a thing
|
||||
class NameMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0x91;
|
||||
/// @brief The length of the message
|
||||
static const unsigned char length = 4;
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief The ID of the thing
|
||||
unsigned char thingId;
|
||||
/// @brief The length of the name, excluding the null terminator
|
||||
unsigned char nameLength;
|
||||
/// @brief The name of the thing, not terminated with a null character
|
||||
const char* name;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId The network ID of the thing
|
||||
/// @param thing The ID of the thing
|
||||
NameMsg(unsigned char networkId, Thing* thing);
|
||||
// NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
|
||||
// unsigned char nameLength);
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
NameMsg(const char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~NameMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,27 +0,0 @@
|
||||
#include "ParticipantMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
ParticipantMsg::ParticipantMsg(char networkId) {
|
||||
this->networkId = networkId;
|
||||
}
|
||||
|
||||
ParticipantMsg::ParticipantMsg(const char* buffer) {
|
||||
this->networkId = buffer[1];
|
||||
}
|
||||
|
||||
ParticipantMsg::~ParticipantMsg() {}
|
||||
|
||||
unsigned char ParticipantMsg::Serialize(char* buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
return ParticipantMsg::length;
|
||||
}
|
||||
|
||||
// bool ParticipantMsg::Send(LocalParticipant *participant, unsigned char networkId) {
|
||||
// ParticipantMsg msg = ParticipantMsg()
|
||||
// }
|
||||
// Client Msg
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,34 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A participant messages notifies other participants of its presence
|
||||
/// When received by another participant, it can be followed by a NetworkIdMsg
|
||||
/// to announce that participant to this client such that it can join privately
|
||||
class ParticipantMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0xA0;
|
||||
/// @brief The length of the message
|
||||
static const unsigned char length = 2;
|
||||
/// @brief The network ID known by the participant
|
||||
unsigned char networkId;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId The network ID known by the participant
|
||||
ParticipantMsg(char networkId);
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
ParticipantMsg(const char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~ParticipantMsg();
|
||||
|
||||
/// @brief Serialize the message into a byte array
|
||||
/// @param buffer The buffer to serialize into
|
||||
/// @return The length of the message in the buffer
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,66 +0,0 @@
|
||||
#include "PoseMsg.h"
|
||||
#include "LowLevelMessages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thing->id;
|
||||
|
||||
this->poseType = 0;
|
||||
if (thing->positionUpdated || force) {
|
||||
this->position = thing->GetPosition();
|
||||
this->poseType |= Pose_Position;
|
||||
thing->positionUpdated = false;
|
||||
}
|
||||
if (thing->orientationUpdated || force) {
|
||||
this->orientation = thing->GetOrientation();
|
||||
this->poseType |= Pose_Orientation;
|
||||
thing->orientationUpdated = false;
|
||||
}
|
||||
if (thing->linearVelocityUpdated) {
|
||||
this->linearVelocity = thing->GetLinearVelocity();
|
||||
this->poseType |= Pose_LinearVelocity;
|
||||
thing->linearVelocityUpdated = false;
|
||||
}
|
||||
if (thing->angularVelocityUpdated) {
|
||||
this->angularVelocity = thing->GetAngularVelocity();
|
||||
this->poseType |= Pose_AngularVelocity;
|
||||
thing->angularVelocityUpdated = false;
|
||||
}
|
||||
}
|
||||
|
||||
PoseMsg::PoseMsg(const char* buffer) {
|
||||
unsigned char ix = 1; // First byte is msg id
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->poseType = buffer[ix++];
|
||||
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
|
||||
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
||||
// linearVelocity
|
||||
// angularVelocity
|
||||
}
|
||||
|
||||
PoseMsg::~PoseMsg() {}
|
||||
|
||||
unsigned char PoseMsg::Serialize(char* buffer) {
|
||||
if (this->poseType == 0)
|
||||
return 0;
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = PoseMsg::id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
buffer[ix++] = this->poseType;
|
||||
if ((this->poseType & Pose_Position) != 0)
|
||||
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
|
||||
if ((this->poseType & Pose_Orientation) != 0)
|
||||
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
||||
if ((this->poseType & Pose_LinearVelocity) != 0)
|
||||
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
|
||||
if ((this->poseType & Pose_AngularVelocity) != 0)
|
||||
LowLevelMessages::SendSpherical(buffer, &ix, this->angularVelocity);
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,55 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message to communicate the pose of the thing
|
||||
/// The pose is in local space relative to the parent. If there is not parent
|
||||
/// (the thing is a root thing), the pose will be in world space.
|
||||
class PoseMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0x10;
|
||||
/// @brief The length of the message
|
||||
unsigned char length = 4 + 4 + 4;
|
||||
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief The ID of the thing
|
||||
unsigned char thingId;
|
||||
|
||||
/// @brief Bit pattern stating which pose components are available
|
||||
unsigned char poseType;
|
||||
/// @brief Bit pattern for a pose with position
|
||||
static const unsigned char Pose_Position = 0x01;
|
||||
/// @brief Bit pattern for a pose with orientation
|
||||
static const unsigned char Pose_Orientation = 0x02;
|
||||
/// @brief Bit pattern for a pose with linear velocity
|
||||
static const unsigned char Pose_LinearVelocity = 0x04;
|
||||
/// @brief Bit pattern for a pose with angular velocity
|
||||
static const unsigned char Pose_AngularVelocity = 0x08;
|
||||
|
||||
/// @brief The position of the thing in local space in meters
|
||||
Spherical position;
|
||||
/// @brief The orientation of the thing in local space
|
||||
SwingTwist orientation;
|
||||
/// @brief The linear velocity of the thing in local space in meters per
|
||||
/// second
|
||||
Spherical linearVelocity;
|
||||
/// @brief The angular velocity of the thing in local space
|
||||
Spherical angularVelocity;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId he network ID of the thing
|
||||
/// @param thing The thing for which the pose shouldbe sent
|
||||
PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
PoseMsg(const char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~PoseMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,22 +0,0 @@
|
||||
#include "SiteMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
SiteMsg::SiteMsg(const char* buffer) {
|
||||
this->networkId = buffer[1];
|
||||
}
|
||||
|
||||
SiteMsg::SiteMsg(unsigned char networkId) {
|
||||
this->networkId = networkId;
|
||||
}
|
||||
|
||||
SiteMsg::~SiteMsg() {}
|
||||
|
||||
unsigned char SiteMsg::Serialize(char* buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
return SiteMsg::length;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,27 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief A message communicating the network ID for that participant
|
||||
class SiteMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0xA1;
|
||||
/// @brief The length of the message
|
||||
static const unsigned char length = 2;
|
||||
/// @brief The network ID for the participant
|
||||
unsigned char networkId;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param networkId The network ID for the participant
|
||||
SiteMsg(unsigned char networkId);
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
SiteMsg(const char *buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~SiteMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char *buffer) override;
|
||||
};
|
||||
|
||||
} // namespace Control
|
||||
@ -1,36 +0,0 @@
|
||||
#include "TextMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
TextMsg::TextMsg(const char* text, unsigned char textLength) {
|
||||
this->text = text;
|
||||
this->textLength = textLength;
|
||||
}
|
||||
|
||||
TextMsg::TextMsg(char* buffer) {
|
||||
unsigned char ix = 1; // first byte is msg id
|
||||
|
||||
this->textLength = buffer[ix++];
|
||||
char* text = new char[this->textLength + 1];
|
||||
for (int i = 0; i < this->textLength; i++)
|
||||
text[i] = buffer[ix++];
|
||||
text[this->textLength] = '\0';
|
||||
this->text = text;
|
||||
}
|
||||
|
||||
TextMsg::~TextMsg() {}
|
||||
|
||||
unsigned char TextMsg::Serialize(char* buffer) {
|
||||
if (this->textLength == 0 || this->text == nullptr)
|
||||
return 0;
|
||||
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->textLength;
|
||||
for (int nameIx = 0; nameIx < this->textLength; nameIx++)
|
||||
buffer[ix++] = this->text[nameIx];
|
||||
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,33 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message for sending generic text
|
||||
class TextMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0xB0;
|
||||
/// @brief The length of the message without the text itself
|
||||
static const unsigned char length = 2;
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief the ID of the thing
|
||||
unsigned char thingId;
|
||||
/// @brief The text without the null terminator
|
||||
const char* text;
|
||||
/// @brief The length of the text
|
||||
unsigned char textLength;
|
||||
|
||||
/// @brief Create a new message for sending
|
||||
/// @param text The text
|
||||
TextMsg(const char* text, unsigned char textLength);
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
TextMsg(char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~TextMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,44 +0,0 @@
|
||||
#include "ThingMsg.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
ThingMsg::ThingMsg(const char* buffer) {
|
||||
unsigned char ix = 1; // first byte is msg id
|
||||
this->networkId = buffer[ix++];
|
||||
this->thingId = buffer[ix++];
|
||||
this->thingType = buffer[ix++];
|
||||
this->parentId = buffer[ix++];
|
||||
}
|
||||
|
||||
ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
|
||||
this->networkId = networkId;
|
||||
this->thingId = thing->id;
|
||||
this->thingType = thing->type;
|
||||
Thing* parent = thing->GetParent();
|
||||
if (parent != nullptr)
|
||||
this->parentId = parent->id;
|
||||
else
|
||||
this->parentId = 0;
|
||||
}
|
||||
|
||||
// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
|
||||
// unsigned char thingType, unsigned char parentId) {
|
||||
// this->networkId = networkId;
|
||||
// this->thingId = thingId;
|
||||
// this->thingType = thingType;
|
||||
// this->parentId = parentId;
|
||||
// }
|
||||
|
||||
ThingMsg::~ThingMsg() {}
|
||||
|
||||
unsigned char ThingMsg::Serialize(char* buffer) {
|
||||
unsigned char ix = 0;
|
||||
buffer[ix++] = this->id;
|
||||
buffer[ix++] = this->networkId;
|
||||
buffer[ix++] = this->thingId;
|
||||
buffer[ix++] = this->thingType;
|
||||
buffer[ix++] = this->parentId;
|
||||
return ix;
|
||||
}
|
||||
|
||||
} // namespace RoboidControl
|
||||
@ -1,37 +0,0 @@
|
||||
#include "Messages.h"
|
||||
|
||||
namespace RoboidControl {
|
||||
|
||||
/// @brief Message providing generic information about a Thing
|
||||
class ThingMsg : public IMessage {
|
||||
public:
|
||||
/// @brief The message ID
|
||||
static const unsigned char id = 0x80;
|
||||
/// @brief The length of the message
|
||||
static const unsigned char length = 5;
|
||||
/// @brief The network ID of the thing
|
||||
unsigned char networkId;
|
||||
/// @brief The ID of the thing
|
||||
unsigned char thingId;
|
||||
/// @brief The Thing.Type of the thing
|
||||
unsigned char thingType;
|
||||
/// @brief The parent of the thing in the hierarachy. This is null for root Things
|
||||
unsigned char parentId;
|
||||
|
||||
/// @brief Create a message for sending
|
||||
/// @param networkId The network ID of the thing</param>
|
||||
/// @param thing The thing
|
||||
ThingMsg(unsigned char networkId, Thing* thing);
|
||||
// ThingMsg(unsigned char networkId, unsigned char thingId,
|
||||
// unsigned char thingType, unsigned char parentId);
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::IMessage(char*)
|
||||
ThingMsg(const char* buffer);
|
||||
/// @brief Destructor for the message
|
||||
virtual ~ThingMsg();
|
||||
|
||||
/// @copydoc RoboidControl::IMessage::Serialize
|
||||
virtual unsigned char Serialize(char* buffer) override;
|
||||
};
|
||||
|
||||
} // namespace RoboidControl
|
||||
32
Motor.cpp
Normal file
32
Motor.cpp
Normal file
@ -0,0 +1,32 @@
|
||||
#include "Motor.h"
|
||||
#include <time.h>
|
||||
|
||||
// #include <Arduino.h>
|
||||
|
||||
Motor::Motor() {
|
||||
type = Thing::MotorType;
|
||||
}
|
||||
|
||||
float Motor::GetSpeed() {
|
||||
return this->currentTargetSpeed;
|
||||
}
|
||||
|
||||
void Motor::SetSpeed(float targetSpeed) {
|
||||
this->currentTargetSpeed = targetSpeed;
|
||||
}
|
||||
|
||||
bool Motor::Drive(float distance) {
|
||||
if (!this->driving) {
|
||||
this->startTime = time(NULL);
|
||||
this->targetDistance = distance >= 0 ? distance : -distance;
|
||||
this->driving = true;
|
||||
}
|
||||
double duration = difftime(time(NULL), this->startTime);
|
||||
if (duration >= this->targetDistance) {
|
||||
this->driving = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
SetSpeed(distance < 0 ? -1 : 1); // max speed
|
||||
return false;
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user