diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index ad8024c292..5a1b5c7c20 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs @@ -319,6 +319,7 @@ public sealed class BSLinksetCompound : BSLinkset // Constraint linksets are rebuilt every time. // Note that this works for rebuilding just the root after a linkset is taken apart. // Called at taint time!! + private bool disableCOM = true; // disable until we get this debugged private void RecomputeLinksetCompound() { try @@ -331,15 +332,26 @@ public sealed class BSLinksetCompound : BSLinkset // The center of mass for the linkset is the geometric center of the group. // Compute a displacement for each component so it is relative to the center-of-mass. - OMV.Vector3 centerOfMass = ComputeLinksetGeometricCenter(); - OMV.Vector3 centerDisplacement = centerOfMass - LinksetRoot.RawPosition; + // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass + OMV.Vector3 centerOfMass; + OMV.Vector3 centerDisplacement = OMV.Vector3.Zero; + if (disableCOM) // DEBUG DEBUG + { // DEBUG DEBUG + centerOfMass = LinksetRoot.RawPosition; // DEBUG DEBUG + LinksetRoot.PositionDisplacement = OMV.Vector3.Zero; + } // DEBUG DEBUG + else + { + centerOfMass = ComputeLinksetGeometricCenter(); + centerDisplacement = centerOfMass - LinksetRoot.RawPosition; - // Since we're displacing the center of the shape, we need to move the body in the world - LinksetRoot.PositionDisplacement = centerDisplacement * LinksetRoot.RawOrientation; + // Since we're displacing the center of the shape, we need to move the body in the world + LinksetRoot.PositionDisplacement = centerDisplacement; - PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false); - DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", - LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement); + PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false); + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", + LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement); + } DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}", LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren); @@ -357,14 +369,15 @@ public sealed class BSLinksetCompound : BSLinkset BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo; if (lci == null) { - // Each child position and rotation is given relative to the root. + // Each child position and rotation is given relative to the center-of-mass. OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation); - OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation; + OMV.Vector3 displacementFromRoot = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation; + OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement; OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation; // Save relative position for recomputing child's world position after moving linkset. - lci = new BSLinksetCompoundInfo(memberIndex, displacementPos, displacementRot); - lci.OffsetFromCenterOfMass = displacementPos - centerDisplacement; + lci = new BSLinksetCompoundInfo(memberIndex, displacementFromCOM, displacementRot); + lci.OffsetFromRoot = displacementFromRoot; cPrim.LinksetInfo = lci; DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci); } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 003dc5458d..e5f7ab71a6 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -297,7 +297,7 @@ public sealed class BSPrim : BSPhysObject */ // don't do the GetObjectPosition for root elements because this function is called a zillion times. - // _position = PhysicsScene.PE.GetObjectPosition2(PhysicsScene.World, BSBody); + // _position = PhysicsScene.PE.GetObjectPosition2(PhysicsScene.World, BSBody) - PositionDisplacement; return _position; } set { @@ -362,7 +362,7 @@ public sealed class BSPrim : BSPhysObject { bool ret = false; - if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position)) + if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(RawPosition)) { // The physical object is out of the known/simulated area. // Upper levels of code will handle the transition to other areas so, for @@ -376,8 +376,11 @@ public sealed class BSPrim : BSPhysObject { DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight); float targetHeight = terrainHeight + (Size.Z / 2f); - // Upforce proportional to the distance away from the terrain. Correct the error in 1 sec. - upForce.Z = (terrainHeight - RawPosition.Z) * 1f; + // If the object is below ground it just has to be moved up because pushing will + // not get it through the terrain + _position.Z = targetHeight; + if (!inTaintTime) + ForcePosition = _position; ret = true; } @@ -389,20 +392,15 @@ public sealed class BSPrim : BSPhysObject { // Upforce proportional to the distance away from the water. Correct the error in 1 sec. upForce.Z = (waterHeight - RawPosition.Z) * 1f; + + // Apply upforce and overcome gravity. + OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity; + DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce); + AddForce(correctionForce, false, inTaintTime); ret = true; } } - // The above code computes a force to apply to correct any out-of-bounds problems. Apply same. - // TODO: This should be intergrated with a geneal physics action mechanism. - // TODO: This should be moderated with PID'ness. - if (ret) - { - // Apply upforce and overcome gravity. - OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity; - DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce); - AddForce(correctionForce, false, inTaintTime); - } return ret; }