Merge branch 'fetures/filter' of git-st.inf.tu-dresden.de:ipos/project into SimpleScene

added SRSConversion

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

@@ -60,11 +60,12 @@ public class Quaternion_math {
     public Quaternion_math set(Vector_math axis, float angle) {
         //matrixs = null;
         angle = (float) Math.toRadians(angle);
-        float s = (float) Math.sin(angle / 2);
-        w = (float) Math.cos(angle / 2);
+        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;
     }
 

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

@@ -29,9 +29,16 @@ public class SRSConversion {
         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());

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

@@ -23,7 +23,7 @@ public class TestSRSConverter {
         Position refPosition_1 = datamodelFactory.createPosition();
         Point3D ref_1 = datamodelFactory.createPoint3D();
         Quaternion rot_1 = datamodelFactory.createQuaternion();
-        Quaternion_math rot_1_math = new Quaternion_math(axis, 45);
+        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());
@@ -59,7 +59,13 @@ public class TestSRSConverter {
         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

@@ -50,11 +50,17 @@ public final class Vector_math {
         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();
-        Quaternion_math tmp1 = new Quaternion_math(x, y, z, 0);
-        tmp2.mulLeft(tmp1).mulLeft(tmp2);
+        tmp1 = new Quaternion_math(x, y, z, 0);
+        tmp2.mulLeft(tmp1);
+        //tmp2.mulLeft(tmp2);
 
         x = tmp2.getX();
         y = tmp2.getY();

src/main/java/ipos/project/Functionality/eventfilter/eventFilter.java

@@ -28,7 +28,7 @@ public class eventFilter {
         return this.monitoringTaskId;
     }
 
-    // kommt später raus, Zugriff auf Weltmodell erfolgt eigentlich über Komponente DataRetrieval
+    // kommt spter raus, Zugriff auf Weltmodell erfolgt eigentlich ber Komponente DataRetrieval
     private static WorldModel wm;
     public static void setWM(WorldModel worldModel){
         wm = worldModel;
@@ -150,6 +150,7 @@ public class eventFilter {
         if (filterStructure[1] && !flag) {
             flag = true; //switch the filter status to block, if the event meet the filter condition, switch to pass
             for (int i = 0; i < categoryCondition.size(); i++) {
+
                 System.out.println("Type in event: " + agent.getAgentType());
 
                 if (agent.getAgentType().equals(categoryCondition.get(i))) {

src/main/java/ipos/project/MainApp.java

 package ipos.project;
 
 import ipos.project.DataModellntegration.SimpleSceneIntegration.SimpleSceneIntegration;
+import ipos.project.Functionality.SRSConversion.TestSRSConverter;
 import ipos.project.Functionality.eventfilter.TestEventFilter;
 import org.springframework.boot.SpringApplication;
 import org.springframework.boot.autoconfigure.SpringBootApplication;
@@ -16,11 +17,14 @@ import java.text.ParseException;
 public class MainApp {
 	public static void main(String[] args) throws ParseException {
 		// TestEventFilter.test();
+		//TestSRSConverter.test();
+
         SpringApplication.run(MainApp.class, args);
         System.out.println("MainApp: Attempting to send mqtt-testmessage");
         SimpleSceneIntegration.mqttService.publish("UWB", "Testnachricht", 0, false);
         SimpleSceneIntegration.mqttService.publish("Human_at_Window_Side", "Testnachricht", 0, false);
         SimpleSceneIntegration.mqttService.publish("Human_at_Door_Side", "Testnachricht", 0, false);
         System.out.println("sent mqtt-testmessage");
+
 	}
 }