aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
blob: bd5ee0b17f76daad20a883324f33290b61ccb1da (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
/*
 * Copyright (c) Contributors, http://opensimulator.org/
 * See CONTRIBUTORS.TXT for a full list of copyright holders.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyrightD
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the OpenSimulator Project nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using OpenSim.Region.Physics.Manager;

using OMV = OpenMetaverse;

namespace OpenSim.Region.Physics.BulletSPlugin
{
public class BSActorAvatarMove : BSActor
{
    BSVMotor m_velocityMotor;

    // Set to true if we think we're going up stairs.
    //    This state is remembered because collisions will turn on and off as we go up stairs.
    int m_walkingUpStairs;
    float m_lastStepUp;

    public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName)
        : base(physicsScene, pObj, actorName)
    {
        m_velocityMotor = null;
        m_walkingUpStairs = 0;
        m_physicsScene.DetailLog("{0},BSActorAvatarMove,constructor", m_controllingPrim.LocalID);
    }

    // BSActor.isActive
    public override bool isActive
    {
        get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
    }

    // Release any connections and resources used by the actor.
    // BSActor.Dispose()
    public override void Dispose()
    {
        Enabled = false;
    }

    // Called when physical parameters (properties set in Bullet) need to be re-applied.
    // Called at taint-time.
    // BSActor.Refresh()
    public override void Refresh()
    {
        m_physicsScene.DetailLog("{0},BSActorAvatarMove,refresh", m_controllingPrim.LocalID);

        // If the object is physically active, add the hoverer prestep action
        if (isActive)
        {
            ActivateAvatarMove();
        }
        else
        {
            DeactivateAvatarMove();
        }
    }

    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
    //     Register a prestep action to restore physical requirements before the next simulation step.
    // Called at taint-time.
    // BSActor.RemoveBodyDependencies()
    public override void RemoveBodyDependencies()
    {
        // Nothing to do for the hoverer since it is all software at pre-step action time.
    }

    // Usually called when target velocity changes to set the current velocity and the target
    //     into the movement motor.
    public void SetVelocityAndTarget(OMV.Vector3 vel, OMV.Vector3 targ, bool inTaintTime)
    {
        m_physicsScene.TaintedObject(inTaintTime, "BSActorAvatarMove.setVelocityAndTarget", delegate()
        {
            if (m_velocityMotor != null)
            {
                m_velocityMotor.Reset();
                m_velocityMotor.SetTarget(targ);
                m_velocityMotor.SetCurrent(vel);
                m_velocityMotor.Enabled = true;
            }
        });
    }

    // If a hover motor has not been created, create one and start the hovering.
    private void ActivateAvatarMove()
    {
        if (m_velocityMotor == null)
        {
            // Infinite decay and timescale values so motor only changes current to target values.
            m_velocityMotor = new BSVMotor("BSCharacter.Velocity", 
                                                0.2f,                       // time scale
                                                BSMotor.Infinite,           // decay time scale
                                                BSMotor.InfiniteVector,     // friction timescale
                                                1f                          // efficiency
            );
            // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
            SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */);

            m_physicsScene.BeforeStep += Mover;

            m_walkingUpStairs = 0;
        }
    }

    private void DeactivateAvatarMove()
    {
        if (m_velocityMotor != null)
        {
            m_physicsScene.BeforeStep -= Mover;
            m_velocityMotor = null;
        }
    }

    // Called just before the simulation step. Update the vertical position for hoverness.
    private void Mover(float timeStep)
    {
        // Don't do movement while the object is selected.
        if (!isActive)
            return;

        // TODO: Decide if the step parameters should be changed depending on the avatar's
        //     state (flying, colliding, ...). There is code in ODE to do this.

        // COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
        //   specified for the avatar is the one that should be used. For falling, if the avatar
        //   is not flying and is not colliding then it is presumed to be falling and the Z
        //   component is not fooled with (thus allowing gravity to do its thing).
        // When the avatar is standing, though, the user has specified a velocity of zero and
        //   the avatar should be standing. But if the avatar is pushed by something in the world
        //   (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
        //   move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
        //   errors can creap in and the avatar will slowly float off in some direction.
        // So, the problem is that, when an avatar is standing, we cannot tell creaping error
        //   from real pushing.
        // The code below uses whether the collider is static or moving to decide whether to zero motion.

        m_velocityMotor.Step(timeStep);
        m_controllingPrim.IsStationary = false;

        // If we're not supposed to be moving, make sure things are zero.
        if (m_velocityMotor.ErrorIsZero() && m_velocityMotor.TargetValue == OMV.Vector3.Zero)
        {
            // The avatar shouldn't be moving
            m_velocityMotor.Zero();

            if (m_controllingPrim.IsColliding)
            {
                // If we are colliding with a stationary object, presume we're standing and don't move around
                if (!m_controllingPrim.ColliderIsMoving)
                {
                    m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
                    m_controllingPrim.IsStationary = true;
                    m_controllingPrim.ZeroMotion(true /* inTaintTime */);
                }

                // Standing has more friction on the ground
                if (m_controllingPrim.Friction != BSParam.AvatarStandingFriction)
                {
                    m_controllingPrim.Friction = BSParam.AvatarStandingFriction;
                    m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
                }
            }
            else
            {
                if (m_controllingPrim.Flying)
                {
                    // Flying and not collising and velocity nearly zero.
                    m_controllingPrim.ZeroMotion(true /* inTaintTime */);
                }
            }

            m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}",
                            m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding);
        }
        else
        {
            // Supposed to be moving.
            OMV.Vector3 stepVelocity = m_velocityMotor.CurrentValue;

            if (m_controllingPrim.Friction != BSParam.AvatarFriction)
            {
                // Probably starting up walking. Set friction to moving friction.
                m_controllingPrim.Friction = BSParam.AvatarFriction;
                m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
            }

            // If falling, we keep the world's downward vector no matter what the other axis specify.
            // The check for RawVelocity.Z < 0 makes jumping work (temporary upward force).
            if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
            {
                if (m_controllingPrim.RawVelocity.Z < 0)
                    stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
                // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
            }

            // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
            OMV.Vector3 moveForce = (stepVelocity - m_controllingPrim.RawVelocity) * m_controllingPrim.Mass;

            // Add special movement force to allow avatars to walk up stepped surfaces.
            moveForce += WalkUpStairs();

            m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}",
                            m_controllingPrim.LocalID, stepVelocity, m_controllingPrim.RawVelocity, m_controllingPrim.Mass, moveForce);
            m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, moveForce);
        }
    }

    // Decide if the character is colliding with a low object and compute a force to pop the
    //    avatar up so it can walk up and over the low objects.
    private OMV.Vector3 WalkUpStairs()
    {
        OMV.Vector3 ret = OMV.Vector3.Zero;

        m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4},avHeight={5}",
                        m_controllingPrim.LocalID, m_controllingPrim.IsColliding, m_controllingPrim.Flying,
                        m_controllingPrim.TargetVelocitySpeed, m_controllingPrim.CollisionsLastTick.Count, m_controllingPrim.Size.Z);
        // This test is done if moving forward, not flying and is colliding with something.
        // Check for stairs climbing if colliding, not flying and moving forward
        if ( m_controllingPrim.IsColliding
                    && !m_controllingPrim.Flying
                    && m_controllingPrim.TargetVelocitySpeed > 0.1f )
        {
            // The range near the character's feet where we will consider stairs
            // float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) + 0.05f;
            // Note: there is a problem with the computation of the capsule height. Thus RawPosition is off
            //    from the height. Revisit size and this computation when height is scaled properly.
            float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) - 0.05f;
            float nearFeetHeightMax = nearFeetHeightMin + BSParam.AvatarStepHeight;

            // Look for a collision point that is near the character's feet and is oriented the same as the charactor is.
            // Find the highest 'good' collision.
            OMV.Vector3 highestTouchPosition = OMV.Vector3.Zero;
            foreach (KeyValuePair<uint, ContactPoint> kvp in m_controllingPrim.CollisionsLastTick.m_objCollisionList)
            {
                // Don't care about collisions with the terrain
                if (kvp.Key > m_physicsScene.TerrainManager.HighestTerrainID)
                {
                    OMV.Vector3 touchPosition = kvp.Value.Position;
                    m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
                                    m_controllingPrim.LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
                    if (touchPosition.Z >= nearFeetHeightMin && touchPosition.Z <= nearFeetHeightMax)
                    {
                        // This contact is within the 'near the feet' range.
                        // The normal should be our contact point to the object so it is pointing away
                        //    thus the difference between our facing orientation and the normal should be small.
                        OMV.Vector3 directionFacing = OMV.Vector3.UnitX * m_controllingPrim.RawOrientation;
                        OMV.Vector3 touchNormal = OMV.Vector3.Normalize(kvp.Value.SurfaceNormal);
                        float diff = Math.Abs(OMV.Vector3.Distance(directionFacing, touchNormal));
                        if (diff < BSParam.AvatarStepApproachFactor)
                        {
                            if (highestTouchPosition.Z < touchPosition.Z)
                                highestTouchPosition = touchPosition;
                        }
                    }
                }
            }
            m_walkingUpStairs = 0;
            // If there is a good step sensing, move the avatar over the step.
            if (highestTouchPosition != OMV.Vector3.Zero)
            {
                // Remember that we are going up stairs. This is needed because collisions
                //    will stop when we move up so this smoothes out that effect.
                m_walkingUpStairs = BSParam.AvatarStepSmoothingSteps;

                m_lastStepUp = highestTouchPosition.Z - nearFeetHeightMin;
                ret = ComputeStairCorrection(m_lastStepUp);
                m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},ret={3}",
                        m_controllingPrim.LocalID, highestTouchPosition, nearFeetHeightMin, ret);
            }
        }
        else
        {
            // If we used to be going up stairs but are not now, smooth the case where collision goes away while
            //    we are bouncing up the stairs.
            if (m_walkingUpStairs > 0)
            {
                m_walkingUpStairs--;
                ret = ComputeStairCorrection(m_lastStepUp);
            }
        }

        return ret;
    }

    private OMV.Vector3 ComputeStairCorrection(float stepUp)
    {
        OMV.Vector3 ret = OMV.Vector3.Zero;
        OMV.Vector3 displacement = OMV.Vector3.Zero;

        if (stepUp > 0f)
        {
            // Found the stairs contact point. Push up a little to raise the character.
            if (BSParam.AvatarStepForceFactor > 0f)
            {
                float upForce = stepUp * m_controllingPrim.Mass * BSParam.AvatarStepForceFactor;
                ret = new OMV.Vector3(0f, 0f, upForce);
            }

            // Also move the avatar up for the new height
            if (BSParam.AvatarStepUpCorrectionFactor > 0f)
            {
                // Move the avatar up related to the height of the collision
                displacement = new OMV.Vector3(0f, 0f, stepUp * BSParam.AvatarStepUpCorrectionFactor);
                m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
            }
            else
            {
                if (BSParam.AvatarStepUpCorrectionFactor < 0f)
                {
                    // Move the avatar up about the specified step height
                    displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight);
                    m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
                }
            }
            m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs.ComputeStairCorrection,disp={1},force={2}",
                                        m_controllingPrim.LocalID, displacement, ret);

        }
        return ret;
    }
}
}