protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove)
        {
        //	printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
	        // phase 2: forward and strafe
	        Matrix start = Matrix.Identity, end = Matrix.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 (Vector3.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.Translation = (m_currentPosition);
		        end.Translation = (m_targetPosition);

		        KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
		        callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
		        callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;


		        float margin = m_convexShape.Margin;
		        m_convexShape.Margin = 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.Margin = margin;

        		
		        fraction -= callback.m_closestHitFraction;

		        if (callback.hasHit())
		        {	
			        // we moved only a fraction
			        float hitDistance = (callback.m_hitPointWorld - m_currentPosition).Length();
			        if (hitDistance<0.0f)
			        {
        //				printf("neg dist?\n");
			        }

			        /* If the distance is farther than the collision margin, move */
			        if (hitDistance > m_addedMargin)
			        {
        //				printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
				        m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
			        }
                    
			        UpdateTargetPositionBasedOnCollision (ref callback.m_hitNormalWorld,0f,1f);
			        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 (Vector3.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;

	        }

        }
        protected void StepDown(CollisionWorld collisionWorld, float dt)
        {
	        Matrix start= Matrix.Identity, end = Matrix.Identity;

	        // phase 3: down
	        Vector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
	        Vector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; 
	        m_targetPosition -= (step_drop + gravity_drop);


	        start.Translation = m_currentPosition;
	        end.Translation = m_targetPosition;

	        KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
	        callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
	        callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
        	
	        if (m_useGhostObjectSweepTest)
	        {
		        m_ghostObject.ConvexSweepTest (m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
	        } 
            else
	        {
                collisionWorld.ConvexSweepTest(m_convexShape, start, end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
	        }

	        if (callback.hasHit())
	        {
		        // we dropped a fraction of the height -> hit floor
		        m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
	        } 
            else 
            {
		        // we dropped the full height
		        m_currentPosition = m_targetPosition;
	        }
        }
        protected void StepUp(CollisionWorld collisionWorld)
        {
	        // phase 1: up
	        Matrix start = Matrix.Identity, end = Matrix.Identity;
	        m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;


	        /* FIXME: Handle penetration properly */
	        start.Translation = (m_currentPosition + upAxisDirection[m_upAxis] * 0.1f);
	        end.Translation = m_targetPosition;

	        KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject);
	        callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
	        callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
        	
	        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,0f);
	        }
        	
	        if (callback.hasHit())
	        {
		        // we moved up only a fraction of the step height
		        m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
                m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
	        } 
            else 
            {
		        m_currentStepOffset = m_stepHeight;
		        m_currentPosition = m_targetPosition;
	        }

        }