boop
Co-authored-by: Damien <damguillotin@gmail.com>
This commit is contained in:
parent
a36a5be8fb
commit
85423171cf
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue