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; package com.tocard.cam;
import org.bukkit.Bukkit;
import org.bukkit.Location; import org.bukkit.Location;
public class LocationQuaternion { public class LocationQuaternion extends Location {
private double[] coordinates = new double[7];
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) { public LocationQuaternion(Location location) {
coordinates[0] = location.getX(); super(location.getWorld(),
coordinates[1] = location.getY(); location.getX(),
coordinates[2] = location.getZ(); location.getY(),
location.getZ(),
location.getYaw() * deg2grad,
location.getPitch() * deg2grad);
double yaw = location.getYaw() * Math.PI / 180; this.updateQuaternionAngles();
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;
} }
public LocationQuaternion(double[] coords) { public void updateEulerAngles() {
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() {
// roll (x-axis rotation) // roll (x-axis rotation)
// double sinr_cosp = 2 * (coordinates[3] * coordinates[4] + coordinates[5] * double sinr_cosp = 2 * (qw * qx + qy * qz);
// coordinates[6]); double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
// double cosr_cosp = 1 - 2 * (coordinates[4] * coordinates[4] + coordinates[5] this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);
// * coordinates[5]);
// double roll = Math.atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation) // pitch (y-axis rotation)
double pitch; double sinp = 2 * (qw * qy - qz * qx);
double sinp = 2 * (coordinates[3] * coordinates[5] - coordinates[6] * coordinates[4]); if (Math.abs(sinp) >= 1) {
if (Math.abs(sinp) >= 1) this.setPitch((float) Math.copySign(Math.PI / 2, sinp)); // use 90 degrees if out of range
pitch = Math.signum(sinp) * Math.PI / 2; // use 90 degrees if out of range } else {
else this.setPitch((float) Math.asin(sinp));
pitch = Math.asin(sinp); }
// yaw (z-axis rotation) // yaw (z-axis rotation)
double siny_cosp = 2 * (coordinates[3] * coordinates[6] + coordinates[4] * coordinates[5]); double siny_cosp = 2 * (qw * qz + qx * qy);
double cosy_cosp = 1 - 2 * (coordinates[5] * coordinates[5] + coordinates[6] * coordinates[6]); double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
double yaw = Math.atan2(siny_cosp, cosy_cosp); this.setYaw((float) 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));
} }
public Location toLocation(double previousYaw) { public void updateQuaternionAngles() {
Location loc = toLocation(); 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; this.qw = cr * cp * cy + sr * sp * sy;
double B = loc.getYaw(); this.qx = sr * cp * cy - cr * sp * sy;
double b = B % 360; this.qy = cr * sp * cy + sr * cp * sy;
int wholePart = 360 * (int) (A / 360); this.qz = cr * cp * sy - sr * sp * cy;
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;
} }
public double dot(LocationQuaternion loc) { public double dot(LocationQuaternion loc) {
double sum = 0; return this.qw * loc.qw + this.qx * loc.qx + this.qy * loc.qy + this.qz * loc.qz;
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;
} }
@Override @Override
public LocationQuaternion clone() { 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; package com.tocard.cam;
import org.bukkit.Bukkit; import org.bukkit.Bukkit;
import org.bukkit.Location; import org.bukkit.Location;
public class LocationQuaternion2 { public class LocationQuaternion2 extends Location {
public double[] coordinates = new double[5];
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) { public LocationQuaternion2(Location location) {
coordinates[0] = location.getX(); super(location.getWorld(),
coordinates[1] = location.getY(); location.getX(),
coordinates[2] = location.getZ(); location.getY(),
coordinates[3] = location.getYaw(); location.getZ(),
coordinates[4] = location.getPitch(); location.getYaw() * deg2grad,
location.getPitch() * deg2grad);
this.updateQuaternionAngles();
} }
public LocationQuaternion2(double[] coords) { public void updateEulerAngles() {
for (int i = 0; i < coordinates.length; i++) { // roll (x-axis rotation)
coordinates[i] = coords[i]; 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) { public void updateQuaternionAngles() {
for (int i = 0; i < coordinates.length; i++) { double cy = Math.cos(getYaw() * 0.5);
coordinates[i] += lq.coordinates[i]; double sy = Math.sin(getYaw() * 0.5);
} double cp = Math.cos(getPitch() * 0.5);
return this; 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) { this.qw = cr * cp * cy + sr * sp * sy;
for (int i = 0; i < coordinates.length; i++) { this.qx = sr * cp * cy - cr * sp * sy;
coordinates[i] -= lq.coordinates[i]; this.qy = cr * sp * cy + sr * cp * sy;
} this.qz = cr * cp * sy - sr * sp * cy;
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;
} }
@Override @Override
public LocationQuaternion2 clone() { 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;
} }
} }