From 707e62816d85b2c951ce13a9e9e5925162464be2 Mon Sep 17 00:00:00 2001
From: "nor..67" <normen667@gmail.com>
Date: Tue, 5 Feb 2013 04:38:30 +0000
Subject: [PATCH] - add setlocation helper method to AbstractPhysicsControl

git-svn-id: https://jmonkeyengine.googlecode.com/svn/trunk@10333 75d07b2b-3a1a-0410-a2c5-0572b91ccdca
---
 .../control/AbstractPhysicsControl.java       | 36 +++++++++++++++----
 1 file changed, 29 insertions(+), 7 deletions(-)

diff --git a/engine/src/bullet-common/com/jme3/bullet/control/AbstractPhysicsControl.java b/engine/src/bullet-common/com/jme3/bullet/control/AbstractPhysicsControl.java
index 5893b2046..fde0f73aa 100644
--- a/engine/src/bullet-common/com/jme3/bullet/control/AbstractPhysicsControl.java
+++ b/engine/src/bullet-common/com/jme3/bullet/control/AbstractPhysicsControl.java
@@ -51,6 +51,7 @@ import java.io.IOException;
  */
 public abstract class AbstractPhysicsControl implements PhysicsControl {
 
+    private final Quaternion tmp_inverseWorldRotation = new Quaternion();
     protected Spatial spatial;
     protected boolean enabled = true;
     protected boolean added = false;
@@ -103,20 +104,46 @@ public abstract class AbstractPhysicsControl implements PhysicsControl {
         applyLocal = applyPhysicsLocal;
     }
 
-    private Vector3f getSpatialTranslation() {
+    protected Vector3f getSpatialTranslation() {
         if (applyLocal) {
             return spatial.getLocalTranslation();
         }
         return spatial.getWorldTranslation();
     }
 
-    private Quaternion getSpatialRotation() {
+    protected Quaternion getSpatialRotation() {
         if (applyLocal) {
             return spatial.getLocalRotation();
         }
         return spatial.getWorldRotation();
     }
 
+    /**
+     * Applies a physics location to the spatial
+     *
+     * @param worldLocation
+     * @param worldRotation
+     */
+    protected void applyPhysicsLocation(Vector3f worldLocation, Quaternion worldRotation) {
+        if (enabled && spatial != null) {
+            Vector3f localLocation = spatial.getLocalTranslation();
+            Quaternion localRotationQuat = spatial.getLocalRotation();
+            if (!applyLocal && spatial.getParent() != null) {
+                localLocation.set(worldLocation).subtractLocal(spatial.getParent().getWorldTranslation());
+                localLocation.divideLocal(spatial.getParent().getWorldScale());
+                tmp_inverseWorldRotation.set(spatial.getParent().getWorldRotation()).inverseLocal().multLocal(localLocation);
+                tmp_inverseWorldRotation.set(spatial.getParent().getWorldRotation()).inverseLocal().mult(localRotationQuat, localRotationQuat);
+
+                spatial.setLocalTranslation(localLocation);
+                spatial.setLocalRotation(localRotationQuat);
+            } else {
+                spatial.setLocalTranslation(worldLocation);
+                spatial.setLocalRotation(worldRotation);
+            }
+        }
+
+    }
+
     public void setSpatial(Spatial spatial) {
         this.spatial = spatial;
         if (spatial == null) {
@@ -148,11 +175,6 @@ public abstract class AbstractPhysicsControl implements PhysicsControl {
     }
 
     public void update(float tpf) {
-        if (!enabled) {
-            return;
-        }
-        setPhysicsLocation(getSpatialTranslation());
-        setPhysicsRotation(getSpatialRotation());
     }
 
     public void render(RenderManager rm, ViewPort vp) {