- added SRSConversion

src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java

+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))));
+    }
+}

src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java

+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;
+    }
+}

src/main/java/ipos/project/Functionality/SRSConversion/TestSRSConverter.java

+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());
+
+
+
+    }
+}

src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java

+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();
+    }
+}