package me.drton.jmavsim;

import me.drton.jmavsim.vehicle.AbstractVehicle;

import javax.vecmath.Matrix3d;
import java.util.List;

/**
 * User: ton Date: 21.03.14 Time: 23:22
 */
public class CameraGimbal2D extends KinematicObject {
    private DynamicObject baseObject;
    private int pitchChannel = -1;
    private double pitchScale = 1.0;
    private int rollChannel = -1;
    private double rollScale = 1.0;

    public CameraGimbal2D(World world) {
        super(world);
    }

    public void setBaseObject(DynamicObject object) {
        this.baseObject = object;
    }

    public void setPitchChannel(int pitchChannel) {
        this.pitchChannel = pitchChannel;
    }

    public void setPitchScale(double pitchScale) {
        this.pitchScale = pitchScale;
    }

    public void setRollChannel(int channel) {
        this.rollChannel = channel;
    }

    public void setRollScale(double scale) {
        this.rollScale = scale;
    }

    @Override
    public void update(long t) {
        this.position = baseObject.position;
        this.velocity = baseObject.velocity;
        this.acceleration = baseObject.acceleration;
        double yaw = Math.atan2(baseObject.rotation.getElement(1, 0), baseObject.rotation.getElement(0, 0));
        this.rotation.rotZ(yaw);
        if ((pitchChannel >= 0 || rollChannel >= 0) && baseObject instanceof AbstractVehicle) {
            // Control camera pitch/roll
            List<Double> control = ((AbstractVehicle) baseObject).getControl();
            Matrix3d r = new Matrix3d();
            if (pitchChannel >= 0 && control.size() > pitchChannel)
                r.rotY(control.get(pitchChannel) * pitchScale);
            if (rollChannel >= 0 && control.size() > rollChannel)
                r.rotX(control.get(rollChannel) * rollScale);
            this.rotation.mul(r);
        }
    }
}

@@ -101,16 +101,48 @@
 trix3d;%0A
+import javax.vecmath.Vector3d;%0A%0A
 import j
@@ -257,16 +257,43 @@
 cObject 
+implements ReportingObject 
 %7B%0A    pr
@@ -465,16 +465,64 @@
 e = 1.0;
+%0A    private Vector3d attitude = new Vector3d();
 %0A%0A    pu
@@ -576,32 +576,559 @@
 r(world);%0A    %7D%0A
+    %0A    public void report(StringBuilder builder) %7B%0A        builder.append(%22GIMBAL%22);%0A        builder.append(newLine);%0A        builder.append(%22===========%22);%0A        builder.append(newLine);%0A%0A        builder.append(String.format(%22Roll: %25.5f%22, Math.toDegrees(this.attitude.x)));%0A        builder.append(newLine);%0A        builder.append(String.format(%22Pitch: %25.5f%22, Math.toDegrees(this.attitude.y)));%0A        builder.append(newLine);%0A        builder.append(String.format(%22Yaw: %25.5f%22, Math.toDegrees(this.attitude.z)));%0A    %7D%0A    
 %0A    public void
@@ -1779,21 +1779,27 @@
     
-double yaw = 
+this.attitude.setZ(
 Math
@@ -1880,16 +1880,17 @@
 t(0, 0))
+)
 ;%0A      
@@ -1914,11 +1914,23 @@
 otZ(
-yaw
+this.attitude.z
 );%0A 
@@ -2245,32 +2245,34 @@
  %3E pitchChannel)
+ %7B
 %0A               
@@ -2272,20 +2272,32 @@
         
-r.ro
+this.attitude.se
 tY(contr
@@ -2324,32 +2324,125 @@
  * pitchScale);%0A
+                r.rotY(this.attitude.y);%0A                this.rotation.mul(r);%0A            %7D%0A
             if (
@@ -2490,16 +2490,18 @@
 Channel)
+ %7B
 %0A       
@@ -2509,20 +2509,32 @@
         
-r.ro
+this.attitude.se
 tX(contr
@@ -2559,32 +2559,77 @@
 ) * rollScale);%0A
+                r.rotX(this.attitude.x);%0A    
             this
@@ -2638,32 +2638,46 @@
 otation.mul(r);%0A
+            %7D%0A
         %7D%0A    %7D%0A

Fix gimbal pitch not working when roll control is also defined; Fix crash when using gimbal view before any valid control inputs (invalid matrix transform error); Add gimbal attitude report.