VRGrab, VRControllerExample.kt: Fix Grabbing for Nodes with rotated p…

…arent
scenerygraphics · Aug 16, 2021 · 78be8b6 · 78be8b6
1 parent 94eed12
commit 78be8b6
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 8 deletions.
diff --git a/src/main/kotlin/graphics/scenery/controls/behaviours/VRGrab.kt b/src/main/kotlin/graphics/scenery/controls/behaviours/VRGrab.kt
@@ -2,6 +2,7 @@ package graphics.scenery.controls.behaviours
 
 import graphics.scenery.Node
 import graphics.scenery.Scene
+import graphics.scenery.attribute.spatial.Spatial
 import graphics.scenery.controls.OpenVRHMD
 import graphics.scenery.controls.TrackedDeviceType
 import graphics.scenery.controls.TrackerRole
@@ -27,9 +28,19 @@ open class VRGrab(
     protected val multiTarget: Boolean = false
 ) : DragBehaviour {
 
+    val controllerSpatial: Spatial
+
+    init {
+        controllerSpatial = controllerHitbox.spatialOrNull()
+            ?: throw IllegalArgumentException("controller hitbox needs a spatial attribute")
+    }
+
     var selected = emptyList<Node>()
     var startPos = Vector3f()
 
+    var lastPos = Vector3f()
+    var lastRotation = Quaternionf()
+
     override fun init(x: Int, y: Int) {
         selected = targets().filter { box -> controllerHitbox.spatialOrNull()?.intersects(box) ?: false }
         if (!multiTarget) {
@@ -47,25 +58,43 @@ open class VRGrab(
             }
         }
         startPos = controllerHitbox.spatialOrNull()?.worldPosition() ?: Vector3f()
+        lastPos = controllerSpatial.worldPosition()
+        lastRotation = controllerSpatial.worldRotation()
     }
 
     override fun drag(x: Int, y: Int) {
         val newPos = controllerHitbox.spatialOrNull()?.worldPosition() ?: Vector3f()
-        val diff = newPos - startPos
-
+        val diffTranslation = newPos - lastPos
+        val diffRotation = Quaternionf(controllerSpatial.worldRotation()).mul(lastRotation.conjugate())
 
         selected.forEach {
             it.ifSpatial {
                 it.getAttributeOrNull(Grabable::class.java)?.let { grabable ->
-                    position = grabable.startPos + diff
+
+                    //apply parent world rotation to diff if available
+                    position += it.parent?.spatialOrNull()?.worldRotation()?.let { q -> diffTranslation.rotate(q) }
+                        ?: diffTranslation
 
                     if (!grabable.lockRotation) {
-                        rotation = Quaternionf(grabable.rotationDiff)
-                        rotation.premul(controllerHitbox.spatialOrNull()?.worldRotation())
+                        it.parent?.spatialOrNull()?.let { pSpatial ->
+                            // if there is a parent spatial
+                            // reverse parent rotation, apply diff rotation, apply parent rotation again
+                            val worldRotationCache = pSpatial.worldRotation()
+                            val newRotation = Quaternionf(worldRotationCache)
+                                .mul(diffRotation)
+                                .mul(worldRotationCache.conjugate())
+
+                            rotation.premul(newRotation)
+                        } ?: let {
+                            rotation.premul(diffRotation)
+                        }
                     }
                 }
             }
         }
+
+        lastPos = controllerSpatial.worldPosition()
+        lastRotation = controllerSpatial.worldRotation()
     }
 
     override fun end(x: Int, y: Int) {
@@ -106,6 +135,10 @@ open class VRGrab(
 }
 
 open class Grabable(val lockRotation: Boolean = false) {
+    internal var lastPos = Vector3f()
+    internal var lastRotation = Vector3f()
+
+
     internal var startPos = Vector3f()
     internal var rotationDiff: Quaternionf? = null
 }
diff --git a/src/main/kotlin/graphics/scenery/controls/behaviours/VRScale.kt b/src/main/kotlin/graphics/scenery/controls/behaviours/VRScale.kt
@@ -30,7 +30,7 @@ class VRScale(
         }
     }
     override fun drag(x: Int, y: Int) {
-        logger.info("both drag OH:${offhand.pressed} Both:${bothPressed}")
+        logger.debug("both drag OH:${offhand.pressed} Both:${bothPressed}")
         if (offhand.pressed && !bothPressed){
             bothPressed = true
             bothInit()
@@ -55,7 +55,7 @@ class VRScale(
     // --- actual behavior ---
     var lastDistance: Float = 0f
     private fun bothInit() {
-        logger.warn("both init")
+        logger.debug("both init")
         lastDistance = controller.worldPosition()
             .distance(offhand.controller.worldPosition())
     }
@@ -69,7 +69,7 @@ class VRScale(
     }
 
     private fun bothEnd() {
-        logger.warn("both end")
+        logger.debug("both end")
         lastDistance = 0f
     }
 

diff --git a/src/test/kotlin/graphics/scenery/tests/examples/advanced/VRControllerExample.kt b/src/test/kotlin/graphics/scenery/tests/examples/advanced/VRControllerExample.kt
@@ -61,6 +61,26 @@ class VRControllerExample : SceneryBase(VRControllerExample::class.java.simpleNa
 
         boxes.forEach { scene.addChild(it) }
 
+        val pivot = RichNode()
+        pivot.spatial().rotation.rotateLocalY(Math.PI.toFloat())
+        scene.addChild(pivot)
+
+        val longBox = Box(Vector3f(0.1f, 0.2f, 0.1f))
+        longBox.spatial {
+            position = Vector3f(-0.5f , 1.0f, 0f)
+        }
+        longBox.addAttribute(Grabable::class.java,Grabable())
+        longBox.addAttribute(Selectable::class.java,Selectable())
+        pivot.addChild(longBox)
+
+
+        (0..5).map {
+            val light = PointLight(radius = 15.0f)
+            light.emissionColor = Random.random3DVectorFromRange(0.0f, 1.0f)
+            light.spatial {
+                position = Random.random3DVectorFromRange(-5.0f, 5.0f)
+            }
+            light.intensity = 1.0f
 
         val lights = Light.createLightTetrahedron<PointLight>(spread = 5.0f, radius = 8.0f)
         lights.forEach {