From 5eddfe791b40850447a662783731ad1779b2ec87 Mon Sep 17 00:00:00 2001
From: Hailong <hailong.zhu@tu-dresden.de>
Date: Fri, 10 Dec 2021 14:52:12 +0100
Subject: [PATCH] - added SRSConversion
---
.../SRSConversion/Quaternion_math.java | 250 ++++++++++++++++++
.../SRSConversion/SRSConversion.java | 89 +++++++
.../SRSConversion/TestSRSConverter.java | 73 +++++
.../SRSConversion/Vector_math.java | 69 +++++
4 files changed, 481 insertions(+)
create mode 100644 src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java
create mode 100644 src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
create mode 100644 src/main/java/ipos/project/Functionality/SRSConversion/TestSRSConverter.java
create mode 100644 src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java b/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java
new file mode 100644
index 0000000..79b2ab5
--- /dev/null
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java
@@ -0,0 +1,250 @@
+package ipos.project.Functionality.SRSConversion;
+
+public class Quaternion_math {
+ private float x;
+ private float y;
+ private float z;
+ private float w;
+ //private float[] matrixs;
+
+ public Quaternion_math(final Quaternion_math q) {
+ this(q.x, q.y, q.z, q.w);
+ }
+
+ public Quaternion_math(float x, float y, float z, float w) {
+ this.x = x;
+ this.y = y;
+ this.z = z;
+ this.w = w;
+ }
+
+ public void set(final Quaternion_math q) {
+ //matrixs = null;
+ this.x = q.x;
+ this.y = q.y;
+ this.z = q.z;
+ this.w = q.w;
+ }
+
+ public Quaternion_math(Vector_math axis, float angle) {
+ set(axis, angle);
+ }
+
+ public float norm() {
+ return (float) Math.sqrt(dot(this));
+ }
+
+ public float getW() {
+ return w;
+ }
+
+ public float getX() {
+ return x;
+ }
+
+ public float getY() {
+ return y;
+ }
+
+ public float getZ() {
+ return z;
+ }
+
+ /**
+ * @param axis
+ * rotation axis, unit vector
+ * @param angle
+ * the rotation angle
+ * @return this
+ */
+ public Quaternion_math set(Vector_math axis, float angle) {
+ //matrixs = null;
+ angle = (float) Math.toRadians(angle);
+ float s = (float) Math.sin(angle);
+ w = (float) Math.cos(angle);
+ x = axis.getX() * s;
+ y = axis.getY() * s;
+ z = axis.getZ() * s;
+ //System.out.println("angle set: " + angle);
+ return this;
+ }
+
+ public Quaternion_math mulThis(Quaternion_math q) {
+ //matrixs = null;
+ float nw = w * q.w - x * q.x - y * q.y - z * q.z;
+ float nx = w * q.x + x * q.w + y * q.z - z * q.y;
+ float ny = w * q.y + y * q.w + z * q.x - x * q.z;
+ z = w * q.z + z * q.w + x * q.y - y * q.x;
+ w = nw;
+ x = nx;
+ y = ny;
+ return this;
+ }
+
+ public Quaternion_math scaleThis(float scale) {
+ if (scale != 1) {
+ //matrixs = null;
+ w *= scale;
+ x *= scale;
+ y *= scale;
+ z *= scale;
+ }
+ return this;
+ }
+
+ public Quaternion_math divThis(float scale) {
+ if (scale != 1) {
+ //matrixs = null;
+ w /= scale;
+ x /= scale;
+ y /= scale;
+ z /= scale;
+ }
+ return this;
+ }
+
+ public float dot(Quaternion_math q) {
+ return x * q.x + y * q.y + z * q.z + w * q.w;
+ }
+
+ public boolean equals(Quaternion_math q) {
+ return x == q.x && y == q.y && z == q.z && w == q.w;
+ }
+
+ public Quaternion_math interpolateThis(Quaternion_math q, float t) {
+ if (!equals(q)) {
+ float d = dot(q);
+ float qx, qy, qz, qw;
+
+ if (d < 0f) {
+ qx = -q.x;
+ qy = -q.y;
+ qz = -q.z;
+ qw = -q.w;
+ d = -d;
+ } else {
+ qx = q.x;
+ qy = q.y;
+ qz = q.z;
+ qw = q.w;
+ }
+
+ float f0, f1;
+
+ if ((1 - d) > 0.1f) {
+ float angle = (float) Math.acos(d);
+ float s = (float) Math.sin(angle);
+ float tAngle = t * angle;
+ f0 = (float) Math.sin(angle - tAngle) / s;
+ f1 = (float) Math.sin(tAngle) / s;
+ } else {
+ f0 = 1 - t;
+ f1 = t;
+ }
+
+ x = f0 * x + f1 * qx;
+ y = f0 * y + f1 * qy;
+ z = f0 * z + f1 * qz;
+ w = f0 * w + f1 * qw;
+ }
+
+ return this;
+ }
+
+ public Quaternion_math inverse() {
+ float norm = norm();
+ if (norm > 0.0) {
+ float invNorm = 1.0f / norm;
+ return new Quaternion_math(-x * invNorm, -y * invNorm, -z * invNorm, w
+ * invNorm);
+ }
+ // return an invalid result to flag the error
+ return null;
+ }
+ public Quaternion_math normalizeThis() {
+ return divThis(norm());
+ }
+
+ public Quaternion_math interpolate(Quaternion_math q, float t) {
+ return new Quaternion_math(this).interpolateThis(q, t);
+ }
+
+
+
+
+ /**
+ * Converts this Quaternion_math into a matrix, returning it as a float array.
+ */
+ public float[] toMatrix() {
+ float[] matrixs = new float[16];
+ toMatrix(matrixs);
+ return matrixs;
+ }
+ /** Conjugate the Quaternion_math.
+ *
+ * @return This Quaternion_math for chaining */
+ public Quaternion_math conjugate () {
+ x = -x;
+ y = -y;
+ z = -z;
+ return this;
+ }
+
+ /** Multiplies this Quaternion_math with another one
+ *
+ * @param q Quaternion_math to multiply with
+ * @return This Quaternion_math for chaining */
+ public Quaternion_math mul (Quaternion_math q) {
+ float newX = w * q.x + x * q.w + y * q.z - z * q.y;
+ float newY = w * q.y + y * q.w + z * q.x - x * q.z;
+ float newZ = w * q.z + z * q.w + x * q.y - y * q.x;
+ float newW = w * q.w - x * q.x - y * q.y - z * q.z;
+ x = newX;
+ y = newY;
+ z = newZ;
+ w = newW;
+ return this;
+ }
+
+ /** Multiplies this Quaternion_math with another one in the form of q * this
+ *
+ * @param q Quaternion_math to multiply with
+ * @return This Quaternion_math for chaining */
+ public Quaternion_math mulLeft (Quaternion_math q) {
+ float newX = q.w * x + q.x * w + q.y * z - q.z * y;
+ float newY = q.w * y + q.y * w + q.z * x - q.x * z;
+ float newZ = q.w * z + q.z * w + q.x * y - q.y * x;
+ float newW = q.w * w - q.x * x - q.y * y - q.z * z;
+ x = newX;
+ y = newY;
+ z = newZ;
+ w = newW;
+ return this;
+ }
+
+ /**
+ * Converts this Quaternion_math into a matrix, placing the values into the given array.
+ * @param matrixs 16-length float array.
+ */
+ public final void toMatrix(float[] matrixs) {
+ matrixs[3] = 0.0f;
+ matrixs[7] = 0.0f;
+ matrixs[11] = 0.0f;
+ matrixs[12] = 0.0f;
+ matrixs[13] = 0.0f;
+ matrixs[14] = 0.0f;
+ matrixs[15] = 1.0f;
+
+ matrixs[0] = (float) (1.0f - (2.0f * ((y * y) + (z * z))));
+ matrixs[1] = (float) (2.0f * ((x * y) - (z * w)));
+ matrixs[2] = (float) (2.0f * ((x * z) + (y * w)));
+
+ matrixs[4] = (float) (2.0f * ((x * y) + (z * w)));
+ matrixs[5] = (float) (1.0f - (2.0f * ((x * x) + (z * z))));
+ matrixs[6] = (float) (2.0f * ((y * z) - (x * w)));
+
+ matrixs[8] = (float) (2.0f * ((x * z) - (y * w)));
+ matrixs[9] = (float) (2.0f * ((y * z) + (x * w)));
+ matrixs[10] = (float) (1.0f - (2.0f * ((x * x) + (y * y))));
+ }
+}
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java b/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
new file mode 100644
index 0000000..7962b92
--- /dev/null
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
@@ -0,0 +1,89 @@
+package ipos.project.Functionality.SRSConversion;
+import org.eclipse.emf.common.util.EList;
+
+import ipos.project.DataModellntegration.iPos_Datamodel.*;
+
+/*
+This class contains methods to convert the coordinate system of positions.
+All reference Systems should be defined in the ROOT system.
+*/
+public class SRSConversion {
+ /*
+ This function converts the coordinate system of a position to a desired coordinate system.
+ p1 is the coordinate of the position in the original coordinate system
+ ref_1 is the origin point of the original coordinate system (A Point3D in ROOT)
+ ref_2 is the origin point of the desired coordinate system (A Point3D in ROOT)
+ quat_1 is the rotation of the original coordinate system (against ROOT)
+ quat_2 is the rotation of the desired coordinate system (against ROOT)
+ */
+ public static void positionInOtherSystem(Position pos, ReferenceSystem ref){
+ IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
+
+ Point3D P = (Point3D) pos.getPoint();
+ Point3D R1 = (Point3D) pos.getReferenceSystem().getOrigin().getPosition().getPoint();
+ Point3D R2 = (Point3D) ref.getOrigin().getPosition().getPoint();
+ Quaternion Q1 = (Quaternion) pos.getReferenceSystem().getOrigin().getOrientation();
+ Quaternion Q2 = (Quaternion) ref.getOrigin().getOrientation();
+
+ Vector_math p1 = new Vector_math(P.getX(), P.getY(), P.getZ());
+ Vector_math ref_1 = new Vector_math(R1.getX(), R1.getY(), R1.getZ());
+ Vector_math ref_2 = new Vector_math(R2.getX(), R2.getY(), R2.getZ());
+ Quaternion_math quat_1 = new Quaternion_math(Q1.getX(), Q1.getY(), Q1.getZ(), Q1.getW());
+ Quaternion_math quat_2 = new Quaternion_math(Q2.getX(), Q2.getY(), Q2.getZ(), Q2.getW());
+ //System.out.println("Quaterion_1: " + quat_1.getX() + ", "+ quat_1.getY() + ", "+ quat_1.getZ() + ", " + quat_1.getW());
+ //System.out.println("Quaterion_2: " + quat_2.getX() + ", "+ quat_2.getY() + ", "+ quat_2.getZ() + ", " + quat_2.getW());
+
+ //System.out.println("inversed Quaterion_1: " + quat_1.inverse().getX() + ", "+ quat_1.inverse().getY() + ", "+ quat_1.inverse().getZ() + ", " + quat_1.inverse().getW());
+ p1.transform(quat_1.inverse());
+
+ //System.out.println("phase 1 transfrom: " + p1.getX() + ", "+ p1.getY() + ", "+ p1.getZ());
+ p1.transform(quat_2);
+ p1.add(ref_1);
+ p1.substract(ref_2);
+
+ P.setX(p1.getX());
+ P.setY(p1.getY());
+ P.setZ(p1.getZ());
+ pos.setPoint(P);
+ pos.setReferenceSystem(ref);
+ }
+ public static void zoneInOtherSystem(Zone zone, ReferenceSystem ref){
+ IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
+ EList spaceList = zone.getSpace();
+ for (int i = 0; i < spaceList.size(); i++) {
+ Space S = (Space)spaceList.get(i);
+ Placing P = S.getCentrePoint();
+ Position Pos = P.getPosition();
+ positionInOtherSystem(Pos, ref);
+ Quaternion Q1 = (Quaternion) P.getOrientation();
+ Quaternion Q2 = (Quaternion) ref.getOrigin().getOrientation();
+
+ Quaternion_math quat_1 = new Quaternion_math(Q1.getX(), Q1.getY(), Q1.getZ(), Q1.getW());
+ Quaternion_math quat_2 = new Quaternion_math(Q2.getX(), Q2.getY(), Q2.getZ(), Q2.getW());
+ quat_1.mulLeft(quat_2);
+ Q1.setX(quat_1.getX());
+ Q1.setY(quat_1.getY());
+ Q1.setZ(quat_1.getZ());
+ Q1.setW(quat_1.getW());
+ P.setOrientation(Q1);
+ P.setPosition(Pos);
+ S.setCentrePoint(P);
+ spaceList.set(i, S);
+ }
+ }
+ public float[] getCordInOtherSystem (float[] cod, float[] ref, float[] quaternion){
+ float[] new_cod = new float[3];
+ //float[] conv_vec = new float[16];
+ Vector_math cod_vec = new Vector_math(cod[0], cod[1], cod[2]);
+ Vector_math ref_new = new Vector_math(ref[0], ref[1], ref[2]);
+ Quaternion_math quaternion_new = new Quaternion_math(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
+ //conv_vec = quaternion_new.toMatrix();
+ //float[][] conv_matrix = new float[][]{{conv_vec[0], conv_vec[1], conv_vec[2]}, {conv_vec[3], conv_vec[4], conv_vec[5]}, {conv_vec[6], conv_vec[7], conv_vec[8]}};
+ cod_vec.transform(quaternion_new);
+ cod_vec.add(ref_new);
+ new_cod[0] = cod_vec.getX();
+ new_cod[1] = cod_vec.getY();
+ new_cod[2] = cod_vec.getZ();
+ return new_cod;
+ }
+}
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/TestSRSConverter.java b/src/main/java/ipos/project/Functionality/SRSConversion/TestSRSConverter.java
new file mode 100644
index 0000000..0407054
--- /dev/null
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/TestSRSConverter.java
@@ -0,0 +1,73 @@
+package ipos.project.Functionality.SRSConversion;
+
+import ipos.project.DataModellntegration.iPos_Datamodel.*;
+import ipos.project.Functionality.SRSConversion.SRSConversion;
+
+import java.sql.Timestamp;
+import java.text.ParseException;
+import java.text.SimpleDateFormat;
+
+public class TestSRSConverter {
+ public static void test() throws ParseException {
+ IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
+ Vector_math axis = new Vector_math(1, 0,0);
+ Point3D testPoint3D = datamodelFactory.createPoint3D();
+ Position testPosition = datamodelFactory.createPosition();
+
+ testPoint3D.setX((float) 0);
+ testPoint3D.setY((float) 1);
+ testPoint3D.setZ((float) 0);
+
+ ReferenceSystem testRef_1 = datamodelFactory.createReferenceSystem();
+ Placing refPlacing_1 = datamodelFactory.createPlacing();
+ Position refPosition_1 = datamodelFactory.createPosition();
+ Point3D ref_1 = datamodelFactory.createPoint3D();
+ Quaternion rot_1 = datamodelFactory.createQuaternion();
+ Quaternion_math rot_1_math = new Quaternion_math(axis, 90);
+ rot_1.setX(rot_1_math.getX());
+ rot_1.setY(rot_1_math.getY());
+ rot_1.setZ(rot_1_math.getZ());
+ rot_1.setW(rot_1_math.getW());
+ ref_1.setX(1);
+ ref_1.setY(1);
+ ref_1.setZ(1);
+ refPosition_1.setPoint(ref_1);
+ refPlacing_1.setPosition(refPosition_1);
+ refPlacing_1.setOrientation(rot_1);
+ testRef_1.setId("CITI");
+ testRef_1.setOrigin(refPlacing_1);
+ testPosition.setPoint(testPoint3D);
+ testPosition.setReferenceSystem(testRef_1);
+
+
+ ReferenceSystem testRef_2 = datamodelFactory.createReferenceSystem();
+ Placing refPlacing_2 = datamodelFactory.createPlacing();
+ Position refPosition_2 = datamodelFactory.createPosition();
+ Point3D ref_2 = datamodelFactory.createPoint3D();
+ Quaternion rot_2 = datamodelFactory.createQuaternion();
+ Quaternion_math rot_2_math = new Quaternion_math(axis, 0);
+ rot_2.setX(rot_2_math.getX());
+ rot_2.setY(rot_2_math.getY());
+ rot_2.setZ(rot_2_math.getZ());
+ rot_2.setW(rot_2_math.getW());
+ ref_2.setX(0);
+ ref_2.setY(0);
+ ref_2.setZ(0);
+ refPosition_2.setPoint(ref_2);
+ refPlacing_2.setPosition(refPosition_2);
+ refPlacing_2.setOrientation(rot_2);
+ testRef_2.setId("ROOT");
+ testRef_2.setOrigin(refPlacing_2);
+
+
+ System.out.println("Position: refID " + ": " + testPosition.getReferenceSystem().getId());
+ System.out.println("Position: coord " + ": " + testPosition.getPoint());
+
+ SRSConversion.positionInOtherSystem(testPosition, testRef_2);
+ System.out.println("Position: refID " + ": " + testPosition.getReferenceSystem().getId());
+ System.out.println("Position: coord " + ": " + testPosition.getPoint());
+
+
+
+ }
+}
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java b/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java
new file mode 100644
index 0000000..d43b7d7
--- /dev/null
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java
@@ -0,0 +1,69 @@
+package ipos.project.Functionality.SRSConversion;
+
+public final class Vector_math {
+ private static Quaternion_math tmp1 = new Quaternion_math(0, 0, 0, 0);
+ private static Quaternion_math tmp2 = new Quaternion_math(0, 0, 0, 0);
+ private float x,y,z;
+
+ public float getX() {
+ return x;
+ }
+
+ public float getY() {
+ return y;
+ }
+
+ public float getZ() {
+ return z;
+ }
+
+ public Vector_math(float ix, float iy, float iz) {
+ x = ix;
+ y = iy;
+ z = iz;
+ }
+
+ public void set(float ix, float iy, float iz) {
+ x = ix;
+ y = iy;
+ z = iz;
+ }
+
+ public double magnitude() {
+ return Math.sqrt(x*x+y*y+z*z);
+ }
+
+ public void multiply(float f) {
+ x *= f;
+ y *= f;
+ z *= f;
+ }
+
+ public void normalise() {
+ double mag = magnitude();
+ x /= mag;
+ y /= mag;
+ z /= mag;
+ }
+ public void add(Vector_math v) {
+ x += v.x;
+ y += v.y;
+ z += v.z;
+ }
+ public void substract(Vector_math v) {
+ x -= v.x;
+ y -= v.y;
+ z -= v.z;
+ }
+ public void transform (Quaternion_math q) {
+ tmp2.set(q);
+ tmp2.conjugate();
+ tmp1 = new Quaternion_math(x, y, z, 0);
+ tmp2.mulLeft(tmp1);
+ //tmp2.mulLeft(tmp2);
+
+ x = tmp2.getX();
+ y = tmp2.getY();
+ z = tmp2.getZ();
+ }
+}
--
GitLab