protected void StepDown(CollisionWorld collisionWorld, float dt)
        {
            Matrix start, end, end_double;
            bool   runonce = false;

            // phase 3: down

            /*float additionalDownStep = (m_wasOnGround && !OnGround) ? m_stepHeight : 0;
             * Vector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
             * float downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity < 0 ? -m_verticalVelocity : 0) * dt;
             * Vector3 gravity_drop = m_up * downVelocity;
             * m_targetPosition -= (step_drop + gravity_drop);*/

            Vector3 orig_position = m_targetPosition;

            float downVelocity = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt;

            if (m_verticalVelocity > 0.0)
            {
                return;
            }

            if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
            {
                downVelocity = m_fallSpeed;
            }

            Vector3 step_drop = m_up * (m_currentStepOffset + downVelocity);

            m_targetPosition -= step_drop;

            using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, m_up, m_maxSlopeCosine))
                using (KinematicClosestNotMeConvexResultCallback callback2 = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, m_up, m_maxSlopeCosine))
                {
                    callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
                    callback.CollisionFilterMask  = GhostObject.BroadphaseHandle.CollisionFilterMask;

                    callback2.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
                    callback2.CollisionFilterMask  = GhostObject.BroadphaseHandle.CollisionFilterMask;

                    while (true)
                    {
                        Matrix.Translation(ref m_currentPosition, out start);
                        Matrix.Translation(ref m_targetPosition, out end);

                        start.SetRotation(m_currentOrientation, out start);
                        end.SetRotation(m_targetOrientation, out end);

                        //set double test for 2x the step drop, to check for a large drop vs small drop
                        end_double = Matrix.Translation(m_targetPosition - step_drop);

                        if (m_useGhostObjectSweepTest)
                        {
                            m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);

                            if (!callback.HasHit && m_ghostObject.HasContactResponse)
                            {
                                //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
                                m_ghostObject.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
                            }
                        }
                        else
                        {
                            collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);

                            if (!callback.HasHit && m_ghostObject.HasContactResponse)
                            {
                                //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
                                collisionWorld.ConvexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld.DispatchInfo.AllowedCcdPenetration);
                            }
                        }

                        float downVelocity2 = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt;
                        bool  has_hit;
                        if (bounce_fix == true)
                        {
                            has_hit = (callback.HasHit || callback2.HasHit) && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject);
                        }
                        else
                        {
                            has_hit = callback2.HasHit && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback2.HitCollisionObject);
                        }

                        float stepHeight = 0.0f;
                        if (m_verticalVelocity < 0.0)
                        {
                            stepHeight = m_stepHeight;
                        }

                        if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping))
                        {
                            //redo the velocity calculation when falling a small amount, for fast stairs motion
                            //for larger falls, use the smoother/slower interpolated movement by not touching the target position

                            m_targetPosition = orig_position;
                            downVelocity     = stepHeight;

                            step_drop         = m_up * (m_currentStepOffset + downVelocity);
                            m_targetPosition -= step_drop;
                            runonce           = true;
                            continue; //re-run previous tests
                        }
                        break;
                    }

                    if ((m_ghostObject.HasContactResponse && (callback.HasHit && NeedsCollision(m_ghostObject, callback.HitCollisionObject))) || runonce == true)
                    {
                        // we dropped a fraction of the height -> hit floor
                        float fraction = (m_currentPosition.Y - callback.HitPointWorld.Y) / 2;

                        //System.Console.WriteLine("hitpoint: {0} - pos {1}", callback.HitPointWorld.Y, m_currentPosition.Y);

                        if (bounce_fix == true)
                        {
                            if (full_drop == true)
                            {
                                m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction);
                            }
                            else
                            {
                                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
                                m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, fraction);
                            }
                        }
                        else
                        {
                            m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction);
                        }

                        full_drop = false;

                        m_verticalVelocity = 0.0f;
                        m_verticalOffset   = 0.0f;
                        m_wasJumping       = false;
                    }
                    else
                    {
                        // we dropped the full height

                        full_drop = true;

                        if (bounce_fix == true)
                        {
                            downVelocity = (m_verticalVelocity < 0f ? -m_verticalVelocity : 0f) * dt;
                            if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
                            {
                                m_targetPosition += step_drop; //undo previous target change
                                downVelocity      = m_fallSpeed;
                                step_drop         = m_up * (m_currentStepOffset + downVelocity);
                                m_targetPosition -= step_drop;
                            }
                        }
                        //System.Console.WriteLine("full drop - {0}, {1}", m_currentPosition.Y, m_targetPosition.Y);

                        m_currentPosition = m_targetPosition;
                    }
                }
        }
        protected void StepUp(CollisionWorld world)
        {
            float stepHeight = 0.0f;

            if (m_verticalVelocity < 0.0f)
            {
                stepHeight = m_stepHeight;
            }

            // phase 1: up
            Matrix start, end;

            start = Matrix.Identity;
            end   = Matrix.Identity;

            /* FIX ME: Handle penetration properly */
            start.Origin = m_currentPosition;

            m_targetPosition  = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0f ? m_verticalOffset : 0f));
            m_currentPosition = m_targetPosition;

            end.Origin = m_targetPosition;

            start.SetRotation(m_currentOrientation, out start);
            end.SetRotation(m_targetOrientation, out end);

            using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -m_up, m_maxSlopeCosine))
            {
                callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
                callback.CollisionFilterMask  = GhostObject.BroadphaseHandle.CollisionFilterMask;

                if (m_useGhostObjectSweepTest)
                {
                    m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, world.DispatchInfo.AllowedCcdPenetration);
                }
                else
                {
                    world.ConvexSweepTest(m_convexShape, start, end, callback, world.DispatchInfo.AllowedCcdPenetration);
                }

                if (callback.HasHit && m_ghostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject))
                {
                    // Only modify the position if the hit was a slope and not a wall or ceiling.
                    if (callback.HitNormalWorld.Dot(m_up) > 0.0)
                    {
                        // we moved up only a fraction of the step height
                        m_currentStepOffset = stepHeight * callback.ClosestHitFraction;
                        if (m_interpolateUp == true)
                        {
                            m_currentPosition = Vector3.Lerp(m_currentPosition, m_targetPosition, callback.ClosestHitFraction);
                        }
                        else
                        {
                            m_currentPosition = m_targetPosition;
                        }
                    }

                    Matrix xform = m_ghostObject.WorldTransform;
                    xform.Origin = m_currentPosition;
                    m_ghostObject.WorldTransform = xform;

                    // fix penetration if we hit a ceiling for example
                    int numPenetrationLoops = 0;
                    m_touchingContact = false;
                    while (RecoverFromPenetration(world))
                    {
                        numPenetrationLoops++;
                        m_touchingContact = true;
                        if (numPenetrationLoops > 4)
                        {
                            //System.Console.WriteLine("character could not recover from penetration = " + numPenetrationLoops);
                            break;
                        }
                    }
                    m_targetPosition  = m_ghostObject.WorldTransform.Origin;
                    m_currentPosition = m_targetPosition;

                    if (m_verticalOffset > 0)
                    {
                        m_verticalOffset    = 0.0f;
                        m_verticalVelocity  = 0.0f;
                        m_currentStepOffset = m_stepHeight;
                    }
                }
                else
                {
                    m_currentStepOffset = stepHeight;
                    m_currentPosition   = m_targetPosition;
                }
            }
        }
        protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
        {
            //System.Console.WriteLine("m_normalizedDirection=" + m_normalizedDirection);
            // phase 2: forward and strafe
            Matrix start = Matrix.Identity;
            Matrix end   = Matrix.Identity;

            m_targetPosition = m_currentPosition + walkMove;

            float fraction  = 1.0f;
            float distance2 = (m_currentPosition - m_targetPosition).LengthSquared;
            //System.Console.WriteLine("distance2=" + distance2);

            int maxIter = 10;

            while (fraction > 0.01f && maxIter-- > 0)
            {
                start.Origin = m_currentPosition;
                end.Origin   = m_targetPosition;
                Vector3 sweepDirNegative = m_currentPosition - m_targetPosition;

                start.SetRotation(m_currentOrientation, out start);
                end.SetRotation(m_targetOrientation, out end);

                using (KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0.0f))
                {
                    callback.CollisionFilterGroup = GhostObject.BroadphaseHandle.CollisionFilterGroup;
                    callback.CollisionFilterMask  = GhostObject.BroadphaseHandle.CollisionFilterMask;

                    float margin = m_convexShape.Margin;
                    m_convexShape.Margin = margin + m_addedMargin;

                    if (start != end)
                    {
                        if (m_useGhostObjectSweepTest)
                        {
                            m_ghostObject.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
                        }
                        else
                        {
                            collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration);
                        }
                    }
                    m_convexShape.Margin = margin;

                    fraction -= callback.ClosestHitFraction;

                    if (callback.HasHit && GhostObject.HasContactResponse && NeedsCollision(m_ghostObject, callback.HitCollisionObject))
                    {
                        // we moved only a fraction
                        //float hitDistance = (callback.HitPointWorld - m_currentPosition).Length;

                        //Vector3.Lerp(ref m_currentPosition, ref m_targetPosition, callback.ClosestHitFraction, out m_currentPosition);
                        Vector3 hitNormalWorld = callback.HitNormalWorld;
                        UpdateTargetPositionBasedOnCollision(ref hitNormalWorld);
                        Vector3 currentDir = m_targetPosition - m_currentPosition;
                        distance2 = currentDir.LengthSquared;
                        if (distance2 > MathUtil.SIMD_EPSILON)
                        {
                            currentDir.Normalize();
                            /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
                            if (currentDir.Dot(m_normalizedDirection) <= 0.0f)
                            {
                                break;
                            }
                        }
                        else
                        {
                            //System.Console.WriteLine("currentDir: don't normalize a zero vector");
                            break;
                        }
                    }
                    else
                    {
                        m_currentPosition = m_targetPosition;
                    }
                }
            }
        }
        public void StepForwardAndStrafe(CollisionWorld collisionWorld, ref IndexedVector3 walkMove)
        {
            //	printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
            // phase 2: forward and strafe
            IndexedMatrix start = IndexedMatrix.Identity, end = IndexedMatrix.Identity;

            m_targetPosition = m_currentPosition + walkMove;

            float fraction  = 1.0f;
            float distance2 = (m_currentPosition - m_targetPosition).LengthSquared();

            //	printf("distance2=%f\n",distance2);

            if (m_touchingContact)
            {
                if (IndexedVector3.Dot(m_normalizedDirection, m_touchingNormal) > 0.0f)
                {
                    UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f);
                }
            }

            int maxIter = 10;

            while (fraction > 0.01f && maxIter-- > 0)
            {
                start._origin = (m_currentPosition);
                end._origin   = (m_targetPosition);

                IndexedVector3 sweepDirNegative = m_currentPosition - m_targetPosition;

                KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, sweepDirNegative, 0f);
                callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
                callback.m_collisionFilterMask  = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;


                float margin = m_convexShape.GetMargin();
                m_convexShape.SetMargin(margin + m_addedMargin);


                if (m_useGhostObjectSweepTest)
                {
                    m_ghostObject.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
                }
                else
                {
                    collisionWorld.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
                }

                m_convexShape.SetMargin(margin);


                fraction -= callback.m_closestHitFraction;

                if (callback.HasHit())
                {
                    // we moved only a fraction
                    float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length();

                    UpdateTargetPositionBasedOnCollision(ref callback.m_hitNormalWorld, 0f, 1f);
                    IndexedVector3 currentDir = m_targetPosition - m_currentPosition;
                    distance2 = currentDir.LengthSquared();
                    if (distance2 > MathUtil.SIMD_EPSILON)
                    {
                        currentDir.Normalize();
                        /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
                        if (IndexedVector3.Dot(currentDir, m_normalizedDirection) <= 0.0f)
                        {
                            break;
                        }
                    }
                    else
                    {
                        //				printf("currentDir: don't normalize a zero vector\n");
                        break;
                    }
                }
                else
                {
                    // we moved whole way
                    m_currentPosition = m_targetPosition;
                }

                //	if (callback.m_closestHitFraction == 0.f)
                //		break;
            }
        }