/*******************************************************************************
 * This file is part of TERMINAL RECALL
 * Copyright (c) 2012-2014 Chuck Ritola
 * Part of the jTRFP.org project
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Public License v3.0
 * which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/gpl.html
 * 
 * Contributors:
 *     chuck - initial API and implementation
 ******************************************************************************/

package org.jtrfp.trcl.beh;

import java.lang.ref.WeakReference;

import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.jtrfp.trcl.math.Vect3D;
import org.jtrfp.trcl.obj.WorldObject;

public class FacingObject extends Behavior {
    private WeakReference<WorldObject> target;
    private final double [] work = new double[3];
    private final double [] perp = new double[3];
    private final double [] UP = new double[]{0,1,0};
    @Override
    public void _tick(long tickTimeMillis){
	if(target!=null){
	    final WorldObject parent = getParent();
	    final double [] tPos = target.get().getPosition();
	    final double [] pPos = parent.getPosition();
	    Vect3D.subtract(tPos, pPos, work);
	    parent.setHeading(new Vector3D(Vect3D.normalize(work,work)));
	    Vect3D.cross(work, UP, perp);
	    Vect3D.cross(perp, work, perp);
	    parent.setTop(target.get().getTop());
	}//end if(!null)
    }//end _tick(...)
    /**
     * @return the target
     */
    public WorldObject getTarget() {
        return target.get();
    }
    /**
     * @param target the target to set
     */
    public FacingObject setTarget(WorldObject target) {
        this.target = new WeakReference<WorldObject>(target);
        return this;
    }
}//end FacingObject

@@ -639,16 +639,84 @@
 rence;%0A%0A
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;%0A
 import o
@@ -1096,16 +1096,265 @@
 0,1,0%7D;%0A
+    private final double %5B%5D newTop = new double%5B3%5D;%0A    private Rotation        readingOffset = Rotation.IDENTITY;%0A    private static final Vector3D DEFAULT_HEADING = Vector3D.PLUS_K;%0A    private static final Vector3D DEFAULT_TOP = Vector3D.PLUS_J;%0A
     @Ove
@@ -1569,24 +1569,30 @@
 Position();%0A
+%09    %0A
 %09    Vect3D.
@@ -1628,26 +1628,36 @@
     
-parent.set
+final Vector3D new
 Heading
-(
+ = 
 new 
@@ -1693,17 +1693,16 @@
 k,work))
-)
 ;%0A%09    V
@@ -1764,54 +1764,295 @@
 rk, 
-perp);%0A%09    parent.setTop(target.get().getTop(
+newTop);%0A%09    final Rotation facingRot = %0A%09%09    new Rotation(DEFAULT_HEADING,DEFAULT_TOP,newHeading,new Vector3D(newTop));%0A%09    %0A%09    parent.setHeading(facingRot.applyTo(readingOffset).applyTo(DEFAULT_HEADING));%0A%09    parent.setTop    (facingRot.applyTo(readingOffset).applyTo(DEFAULT_TOP
 ));%0A
@@ -2405,16 +2405,320 @@
 ;%0A    %7D%0A
+    /**%0A     * @return the readingOffset%0A     */%0A    public Rotation getHeadingOffset() %7B%0A        return readingOffset;%0A    %7D%0A    /**%0A     * @param readingOffset the readingOffset to set%0A     */%0A    public void setHeadingOffset(Rotation readingOffset) %7B%0A        this.readingOffset = readingOffset;%0A    %7D%0A
 %7D//end F

Add heading offset support to FacingObject.