Co-authored-by: Damien <damguillotin@gmail.com>
This commit is contained in:
Laureηt 2022-05-16 19:40:05 +02:00
parent a36a5be8fb
commit 85423171cf
No known key found for this signature in database
GPG key ID: D88C6B294FD40994
2 changed files with 127 additions and 185 deletions

View file

@ -1,141 +1,81 @@
package com.tocard.cam;
import org.bukkit.Bukkit;
import org.bukkit.Location;
public class LocationQuaternion {
private double[] coordinates = new double[7];
public class LocationQuaternion extends Location {
private double qw;
private double qx;
private double qy;
private double qz;
private float roll = 0;
private static final float deg2grad = (float) Math.PI / 180.0f;
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code
public LocationQuaternion(Location location) {
coordinates[0] = location.getX();
coordinates[1] = location.getY();
coordinates[2] = location.getZ();
super(location.getWorld(),
location.getX(),
location.getY(),
location.getZ(),
location.getYaw() * deg2grad,
location.getPitch() * deg2grad);
double yaw = location.getYaw() * Math.PI / 180;
double pitch = location.getPitch() * Math.PI / 180;
double roll = 0;
double cy = Math.cos(yaw * 0.5);
double sy = Math.sin(yaw * 0.5);
double cp = Math.cos(pitch * 0.5);
double sp = Math.sin(pitch * 0.5);
double cr = Math.cos(roll * 0.5);
double sr = Math.sin(roll * 0.5);
coordinates[3] = cr * cp * cy + sr * sp * sy;
coordinates[4] = sr * cp * cy - cr * sp * sy;
coordinates[5] = cr * sp * cy + sr * cp * sy;
coordinates[6] = cr * cp * sy - sr * sp * cy;
this.updateQuaternionAngles();
}
public LocationQuaternion(double[] coords) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] = coords[i];
}
}
public LocationQuaternion add(LocationQuaternion lq) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] += lq.coordinates[i];
}
return this;
}
public LocationQuaternion subtract(LocationQuaternion lq) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] -= lq.coordinates[i];
}
return this;
}
public LocationQuaternion add(Location l) {
LocationQuaternion lq = new LocationQuaternion(l);
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] += lq.coordinates[i];
}
return this;
}
public LocationQuaternion multiply(double m) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] *= m;
}
return this;
}
public double distance(LocationQuaternion loc) {
return Math.sqrt(
coordinates[0] * coordinates[0] + coordinates[1] * coordinates[1] + coordinates[2] * coordinates[2]);
}
public Location toLocation() {
public void updateEulerAngles() {
// roll (x-axis rotation)
// double sinr_cosp = 2 * (coordinates[3] * coordinates[4] + coordinates[5] *
// coordinates[6]);
// double cosr_cosp = 1 - 2 * (coordinates[4] * coordinates[4] + coordinates[5]
// * coordinates[5]);
// double roll = Math.atan2(sinr_cosp, cosr_cosp);
double sinr_cosp = 2 * (qw * qx + qy * qz);
double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double pitch;
double sinp = 2 * (coordinates[3] * coordinates[5] - coordinates[6] * coordinates[4]);
if (Math.abs(sinp) >= 1)
pitch = Math.signum(sinp) * Math.PI / 2; // use 90 degrees if out of range
else
pitch = Math.asin(sinp);
double sinp = 2 * (qw * qy - qz * qx);
if (Math.abs(sinp) >= 1) {
this.setPitch((float) Math.copySign(Math.PI / 2, sinp)); // use 90 degrees if out of range
} else {
this.setPitch((float) Math.asin(sinp));
}
// yaw (z-axis rotation)
double siny_cosp = 2 * (coordinates[3] * coordinates[6] + coordinates[4] * coordinates[5]);
double cosy_cosp = 1 - 2 * (coordinates[5] * coordinates[5] + coordinates[6] * coordinates[6]);
double yaw = Math.atan2(siny_cosp, cosy_cosp);
return new Location(
Bukkit.getWorlds().get(0),
coordinates[0], coordinates[1], coordinates[2],
(float) (yaw * 180 / Math.PI), (float) (pitch * 180 / Math.PI));
double siny_cosp = 2 * (qw * qz + qx * qy);
double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
this.setYaw((float) Math.atan2(siny_cosp, cosy_cosp));
}
public Location toLocation(double previousYaw) {
Location loc = toLocation();
public void updateQuaternionAngles() {
double cy = Math.cos(getYaw() * 0.5);
double sy = Math.sin(getYaw() * 0.5);
double cp = Math.cos(getPitch() * 0.5);
double sp = Math.sin(getPitch() * 0.5);
double cr = Math.cos(getRoll() * 0.5);
double sr = Math.sin(getRoll() * 0.5);
double A = previousYaw;
double B = loc.getYaw();
double b = B % 360;
int wholePart = 360 * (int) (A / 360);
double minDist = -1;
double sol = 0;
for (int i = -1; i < 2; i++) {
double val = wholePart + b + i * 360;
double dist = Math.abs(val - A);
if (minDist == -1 || dist < minDist) {
sol = val;
minDist = dist;
}
}
Camera.broadlog("From " + coordinates[3] + " to " + sol);
loc.setYaw((float) sol);
return loc;
this.qw = cr * cp * cy + sr * sp * sy;
this.qx = sr * cp * cy - cr * sp * sy;
this.qy = cr * sp * cy + sr * cp * sy;
this.qz = cr * cp * sy - sr * sp * cy;
}
public double dot(LocationQuaternion loc) {
double sum = 0;
for (int i = 3; i < 7; i++) {
sum += coordinates[i] + loc.coordinates[i];
}
return sum;
}
public LocationQuaternion invertQuaternion(LocationQuaternion previousLocationQuaternion) {
if (dot(previousLocationQuaternion) < 0)
for (int i = 3; i < 7; i++) {
coordinates[i] *= -1;
}
return this;
return this.qw * loc.qw + this.qx * loc.qx + this.qy * loc.qy + this.qz * loc.qz;
}
@Override
public LocationQuaternion clone() {
return new LocationQuaternion(coordinates);
LocationQuaternion cloned = (LocationQuaternion) super.clone();
cloned.roll = this.roll;
cloned.qw = this.qw;
cloned.qx = this.qx;
cloned.qy = this.qy;
cloned.qz = this.qz;
return cloned;
}
public float getRoll() {
return this.roll;
}
}

View file

@ -1,89 +1,91 @@
// double A = previousLocationQuaternion.coordinates[3];
// double B = coordinates[3];
// double b = B % 360;
// int wholePart = 360 * (int) (A / 360);
// double minDist = -1;
// double sol = 0;
// for (int i = -1; i < 2; i++) {
// double val = wholePart + b + i * 360;
// double dist = Math.abs(val - A);
// if (minDist == -1 || dist < minDist) {
// sol = val;
// minDist = dist;
// }
// }
// Camera.broadlog("From " + coordinates[3] + " to " + sol);
// coordinates[3] = sol;
// return this;
package com.tocard.cam;
import org.bukkit.Bukkit;
import org.bukkit.Location;
public class LocationQuaternion2 {
public double[] coordinates = new double[5];
public class LocationQuaternion2 extends Location {
private double qw;
private double qx;
private double qy;
private double qz;
private static final float deg2grad = (float) Math.PI / 180.0f;
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code
public LocationQuaternion2(Location location) {
coordinates[0] = location.getX();
coordinates[1] = location.getY();
coordinates[2] = location.getZ();
coordinates[3] = location.getYaw();
coordinates[4] = location.getPitch();
super(location.getWorld(),
location.getX(),
location.getY(),
location.getZ(),
location.getYaw() * deg2grad,
location.getPitch() * deg2grad);
this.updateQuaternionAngles();
}
public LocationQuaternion2(double[] coords) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] = coords[i];
public void updateEulerAngles() {
// roll (x-axis rotation)
double sinr_cosp = 2 * (qw * qx + qy * qz);
double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (qw * qy - qz * qx);
if (Math.abs(sinp) >= 1) {
this.setPitch((float) Math.copySign(Math.PI / 2, sinp)); // use 90 degrees if out of range
} else {
this.setPitch((float) Math.asin(sinp));
}
// yaw (z-axis rotation)
double siny_cosp = 2 * (qw * qz + qx * qy);
double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
this.setYaw((float) Math.atan2(siny_cosp, cosy_cosp));
}
public LocationQuaternion2 add(LocationQuaternion2 lq) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] += lq.coordinates[i];
}
return this;
}
public void updateQuaternionAngles() {
double cy = Math.cos(getYaw() * 0.5);
double sy = Math.sin(getYaw() * 0.5);
double cp = Math.cos(getPitch() * 0.5);
double sp = Math.sin(getPitch() * 0.5);
double cr = Math.cos(0 * 0.5);
double sr = Math.sin(0 * 0.5);
public LocationQuaternion2 subtract(LocationQuaternion2 lq) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] -= lq.coordinates[i];
}
return this;
}
public LocationQuaternion2 add(Location l) {
LocationQuaternion2 lq = new LocationQuaternion2(l);
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] += lq.coordinates[i];
}
return this;
}
public LocationQuaternion2 multiply(double m) {
for (int i = 0; i < coordinates.length; i++) {
coordinates[i] *= m;
}
return this;
}
public double distance(LocationQuaternion2 loc) {
return Math.sqrt(
coordinates[0] * coordinates[0] + coordinates[1] * coordinates[1] + coordinates[2] * coordinates[2]);
}
public Location toLocation() {
return new Location(
Bukkit.getWorlds().get(0),
coordinates[0], coordinates[1], coordinates[2],
(float) (coordinates[3]), (float) (coordinates[4]));
}
public LocationQuaternion2 invertQuaternion(LocationQuaternion2 previousLocationQuaternion) {
double A = previousLocationQuaternion.coordinates[3];
double B = coordinates[3];
double b = B % 360;
int wholePart = 360 * (int) (A / 360);
double minDist = -1;
double sol = 0;
for (int i = -1; i < 2; i++) {
double val = wholePart + b + i * 360;
double dist = Math.abs(val - A);
if (minDist == -1 || dist < minDist) {
sol = val;
minDist = dist;
}
}
Camera.broadlog("From " + coordinates[3] + " to " + sol);
coordinates[3] = sol;
return this;
this.qw = cr * cp * cy + sr * sp * sy;
this.qx = sr * cp * cy - cr * sp * sy;
this.qy = cr * sp * cy + sr * cp * sy;
this.qz = cr * cp * sy - sr * sp * cy;
}
@Override
public LocationQuaternion2 clone() {
return new LocationQuaternion2(coordinates);
LocationQuaternion2 cloned = (LocationQuaternion2) super.clone();
cloned.qw = this.qw;
cloned.qx = this.qx;
cloned.qy = this.qy;
cloned.qz = this.qz;
return cloned;
}
}