public override void OnGameTick(float dt)
        {
            if (!Active)
            {
                return;
            }

            int   wayPointIndex = Math.Min(waypoints.Count - 1, waypointToReachIndex);
            Vec3d target        = waypoints[wayPointIndex];

            // Due to the nature of gravity and going down slope we sometimes end up at the next waypoint. So lets also test for the next waypoint
            // Doesn't seem to fully fix the issue though
            int   nextwayPointIndex = Math.Min(waypoints.Count - 1, waypointToReachIndex + 1);
            Vec3d nexttarget        = waypoints[nextwayPointIndex];

            // For land dwellers only check horizontal distance
            double sqDistToTarget    = Math.Min(target.SquareDistanceTo(entity.ServerPos.X, entity.ServerPos.Y, entity.ServerPos.Z), target.SquareDistanceTo(entity.ServerPos.X, entity.ServerPos.Y + 1, entity.ServerPos.Z));    // One block above is also ok
            double horsqDistToTarget = target.HorizontalSquareDistanceTo(entity.ServerPos.X, entity.ServerPos.Z);

            double sqDistToNextTarget = Math.Min(nexttarget.SquareDistanceTo(entity.ServerPos.X, entity.ServerPos.Y, entity.ServerPos.Z), nexttarget.SquareDistanceTo(entity.ServerPos.X, entity.ServerPos.Y + 1, entity.ServerPos.Z));       // One block above is also ok

            bool nearHorizontally = horsqDistToTarget < 1;
            bool nearAllDirs      = sqDistToTarget < targetDistance * targetDistance;

            bool nearAllDirsNext = sqDistToNextTarget < targetDistance * targetDistance;

            //float speedMul = 1;// entity.Properties.Habitat == API.Common.EnumHabitat.Land && waypointToReachIndex >= waypoints.Count - 1 ? Math.Min(1, GameMath.Sqrt(horsqDistToTarget)) : 1;
            //Console.WriteLine(speedMul);



            if (nearAllDirs || nearAllDirsNext)
            {
                waypointToReachIndex++;
                if (nearAllDirsNext && wayPointIndex != nextwayPointIndex)
                {
                    waypointToReachIndex++;
                }

                lastWaypointIncTotalMs = entity.World.ElapsedMilliseconds;

                if (waypointToReachIndex >= waypoints.Count)
                {
                    Stop();
                    OnGoalReached?.Invoke();
                    return;
                }
                else
                {
                    target = waypoints[waypointToReachIndex];
                }
            }

            bool stuck =
                (entity.CollidedVertically && entity.Controls.IsClimbing) ||
                (entity.ServerPos.SquareDistanceTo(prevPos) < 0.005 * 0.005) ||  // This used to test motion, but that makes no sense, we want to test if the entity moved, not if it had motion
                (entity.CollidedHorizontally && entity.ServerPos.Motion.Y <= 0) ||
                (nearHorizontally && !nearAllDirs && entity.Properties.Habitat == API.Common.EnumHabitat.Land) ||
                (waypoints.Count > 1 && waypointToReachIndex < waypoints.Count && entity.World.ElapsedMilliseconds - lastWaypointIncTotalMs > 2000) // If it takes more than 2 seconds to reach next waypoint (waypoints are always 1 block apart)
            ;

            prevPos.Set(entity.ServerPos.X, entity.ServerPos.Y, entity.ServerPos.Z);

            stuckCounter = stuck ? (stuckCounter + 1) : 0;

            if (GlobalConstants.OverallSpeedMultiplier > 0 && stuckCounter > 60 / GlobalConstants.OverallSpeedMultiplier)
            {
                //entity.World.SpawnParticles(10, ColorUtil.WhiteArgb, prevPos, prevPos, new Vec3f(0, 0, 0), new Vec3f(0, -1, 0), 1, 1);
                Stop();
                OnStuck?.Invoke();
                return;
            }


            EntityControls controls = entity.MountedOn == null ? entity.Controls : entity.MountedOn.Controls;

            if (controls == null)
            {
                return;
            }

            targetVec.Set(
                (float)(target.X - entity.ServerPos.X),
                (float)(target.Y - entity.ServerPos.Y),
                (float)(target.Z - entity.ServerPos.Z)
                );

            float desiredYaw = 0;

            if (sqDistToTarget >= 0.01)
            {
                desiredYaw = (float)Math.Atan2(targetVec.X, targetVec.Z);
            }



            float yawDist = GameMath.AngleRadDistance(entity.ServerPos.Yaw, desiredYaw);

            entity.ServerPos.Yaw += GameMath.Clamp(yawDist, -curTurnRadPerSec * dt * GlobalConstants.OverallSpeedMultiplier, curTurnRadPerSec * dt * GlobalConstants.OverallSpeedMultiplier);
            entity.ServerPos.Yaw  = entity.ServerPos.Yaw % GameMath.TWOPI;



            double cosYaw = Math.Cos(entity.ServerPos.Yaw);
            double sinYaw = Math.Sin(entity.ServerPos.Yaw);

            controls.WalkVector.Set(sinYaw, GameMath.Clamp(targetVec.Y, -1, 1), cosYaw);
            controls.WalkVector.Mul(movingSpeed * GlobalConstants.OverallSpeedMultiplier);// * speedMul);

            // Make it walk along the wall, but not walk into the wall, which causes it to climb
            if (entity.Properties.RotateModelOnClimb && entity.Controls.IsClimbing && entity.ClimbingOnFace != null && entity.Alive)
            {
                BlockFacing facing = entity.ClimbingOnFace;
                if (Math.Sign(facing.Normali.X) == Math.Sign(controls.WalkVector.X))
                {
                    controls.WalkVector.X = 0;
                }

                if (Math.Sign(facing.Normali.Z) == Math.Sign(controls.WalkVector.Z))
                {
                    controls.WalkVector.Z = 0;
                }
            }

            //   entity.World.SpawnParticles(0.3f, ColorUtil.WhiteAhsl, target, target, new Vec3f(), new Vec3f(), 0.1f, 0.1f, 3f, EnumParticleModel.Cube);


            if (entity.Swimming)
            {
                controls.FlyVector.Set(controls.WalkVector);

                Vec3d pos                 = entity.Pos.XYZ;
                Block inblock             = entity.World.BlockAccessor.GetBlock((int)pos.X, (int)(pos.Y), (int)pos.Z);
                Block aboveblock          = entity.World.BlockAccessor.GetBlock((int)pos.X, (int)(pos.Y + 1), (int)pos.Z);
                float waterY              = (int)pos.Y + inblock.LiquidLevel / 8f + (aboveblock.IsLiquid() ? 9 / 8f : 0);
                float bottomSubmergedness = waterY - (float)pos.Y;

                // 0 = at swim line
                // 1 = completely submerged
                float swimlineSubmergedness = GameMath.Clamp(bottomSubmergedness - ((float)entity.SwimmingOffsetY), 0, 1);
                swimlineSubmergedness = Math.Min(1, swimlineSubmergedness + 0.5f);
                controls.FlyVector.Y  = GameMath.Clamp(controls.FlyVector.Y, 0.02f, 0.04f) * swimlineSubmergedness;


                if (entity.CollidedHorizontally)
                {
                    controls.FlyVector.Y = 0.05f;
                }
            }
        }
Exemple #2
0
        public override void OnGameTick(float dt)
        {
            if (!Active)
            {
                return;
            }


            // For land dwellers only check horizontal distance
            double sqDistToTarget =
                entity.Properties.Habitat == API.Common.EnumHabitat.Land ?
                target.SquareDistanceTo(entity.ServerPos.X, target.Y, entity.ServerPos.Z) :
                target.SquareDistanceTo(entity.ServerPos.X, entity.ServerPos.Y, entity.ServerPos.Z)
            ;


            if (sqDistToTarget < targetDistance * targetDistance)
            {
                Stop();
                OnGoalReached?.Invoke();
                return;
            }

            bool stuck =
                (entity.CollidedVertically && entity.Controls.IsClimbing) ||
                (entity.ServerPos.SquareDistanceTo(prevPos) < 0.005 * 0.005) ||  // This used to test motion, but that makes no sense, we want to test if the entity moved, not if it had motion
                (entity.CollidedHorizontally && entity.ServerPos.Motion.Y <= 0)
            ;

            prevPos.Set(entity.ServerPos.X, entity.ServerPos.Y, entity.ServerPos.Z);

            stuckCounter = stuck ? (stuckCounter + 1) : 0;

            if (GlobalConstants.OverallSpeedMultiplier > 0 && stuckCounter > 20 / GlobalConstants.OverallSpeedMultiplier)
            {
                //entity.World.SpawnParticles(10, ColorUtil.WhiteArgb, prevPos, prevPos, new Vec3f(0, 0, 0), new Vec3f(0, -1, 0), 1, 1);
                Stop();
                OnStuck?.Invoke();
                return;
            }


            EntityControls controls = entity.MountedOn == null ? entity.Controls : entity.MountedOn.Controls;

            if (controls == null)
            {
                return;
            }

            targetVec.Set(
                (float)(target.X - entity.ServerPos.X),
                (float)(target.Y - entity.ServerPos.Y),
                (float)(target.Z - entity.ServerPos.Z)
                );

            float desiredYaw = 0;

            if (sqDistToTarget >= 0.01)
            {
                desiredYaw = (float)Math.Atan2(targetVec.X, targetVec.Z);
            }



            float yawDist = GameMath.AngleRadDistance(entity.ServerPos.Yaw, desiredYaw);

            entity.ServerPos.Yaw += GameMath.Clamp(yawDist, -curTurnRadPerSec * dt * GlobalConstants.OverallSpeedMultiplier, curTurnRadPerSec * dt * GlobalConstants.OverallSpeedMultiplier);
            entity.ServerPos.Yaw  = entity.ServerPos.Yaw % GameMath.TWOPI;



            double cosYaw = Math.Cos(entity.ServerPos.Yaw);
            double sinYaw = Math.Sin(entity.ServerPos.Yaw);

            controls.WalkVector.Set(sinYaw, GameMath.Clamp(targetVec.Y, -1, 1), cosYaw);
            controls.WalkVector.Mul(movingSpeed * GlobalConstants.OverallSpeedMultiplier);

            // Make it walk along the wall, but not walk into the wall, which causes it to climb
            if (entity.Properties.RotateModelOnClimb && entity.Controls.IsClimbing && entity.ClimbingOnFace != null && entity.Alive)
            {
                BlockFacing facing = entity.ClimbingOnFace;
                if (Math.Sign(facing.Normali.X) == Math.Sign(controls.WalkVector.X))
                {
                    controls.WalkVector.X = 0;
                }

                if (Math.Sign(facing.Normali.Z) == Math.Sign(controls.WalkVector.Z))
                {
                    controls.WalkVector.Z = 0;
                }
            }

            //   entity.World.SpawnParticles(0.3f, ColorUtil.WhiteAhsl, target, target, new Vec3f(), new Vec3f(), 0.1f, 0.1f, 3f, EnumParticleModel.Cube);


            if (entity.Swimming)
            {
                controls.FlyVector.Set(controls.WalkVector);
                controls.FlyVector.X *= 0.85f;
                controls.FlyVector.Z *= 0.85f;

                Vec3d pos                 = entity.Pos.XYZ;
                Block inblock             = entity.World.BlockAccessor.GetBlock((int)pos.X, (int)(pos.Y), (int)pos.Z);
                Block aboveblock          = entity.World.BlockAccessor.GetBlock((int)pos.X, (int)(pos.Y + 1), (int)pos.Z);
                float waterY              = (int)pos.Y + inblock.LiquidLevel / 8f + (aboveblock.IsLiquid() ? 9 / 8f : 0);
                float bottomSubmergedness = waterY - (float)pos.Y;

                // 0 = at swim line
                // 1 = completely submerged
                float swimlineSubmergedness = GameMath.Clamp(bottomSubmergedness - ((float)entity.SwimmingOffsetY), 0, 1);
                swimlineSubmergedness = Math.Min(1, swimlineSubmergedness + 0.075f);
                controls.FlyVector.Y  = GameMath.Clamp(controls.FlyVector.Y, 0.002f, 0.004f) * swimlineSubmergedness;


                if (entity.CollidedHorizontally)
                {
                    controls.FlyVector.Y = 0.05f;
                }
            }
        }
        public override void OnGameTick(float dt)
        {
            if (asyncSearchObject != null)
            {
                if (!asyncSearchObject.Finished)
                {
                    return;
                }

                AfterFoundPath();
            }

            if (!Active)
            {
                return;
            }

            bool nearHorizontally = false;
            int  offset           = 0;
            bool nearAllDirs      =
                IsNearTarget(offset++, ref nearHorizontally) ||
                IsNearTarget(offset++, ref nearHorizontally) ||
                IsNearTarget(offset++, ref nearHorizontally)
            ;

            if (nearAllDirs)
            {
                waypointToReachIndex  += offset;
                lastWaypointIncTotalMs = entity.World.ElapsedMilliseconds;
            }

            target = waypoints[Math.Min(waypoints.Count - 1, waypointToReachIndex)];

            bool onlastWaypoint = waypointToReachIndex == waypoints.Count - 1;

            if (waypointToReachIndex >= waypoints.Count)
            {
                Stop();
                OnGoalReached?.Invoke();
                return;
            }

            bool stuck =
                (entity.CollidedVertically && entity.Controls.IsClimbing) ||
                (entity.CollidedHorizontally && entity.ServerPos.Motion.Y <= 0) ||
                (nearHorizontally && !nearAllDirs && entity.Properties.Habitat == EnumHabitat.Land) ||
                (entity.CollidedHorizontally && waypoints.Count > 1 && waypointToReachIndex < waypoints.Count && entity.World.ElapsedMilliseconds - lastWaypointIncTotalMs > 2000)       // If it takes more than 2 seconds to reach next waypoint (waypoints are always 1 block apart)
                //|| (entity.Swimming && false)
            ;
            // This used to test motion, but that makes no sense, we want to test if the entity moved, not if it had motion


            double distsq = prevPrevPos.SquareDistanceTo(prevPos);

            stuck |= (distsq < 0.01 * 0.01) ? (entity.World.Rand.NextDouble() < GameMath.Clamp(1 - distsq * 1.2, 0.1, 0.9)) : false;


            // Test movement progress between two points in 150 millisecond intervalls
            prevPosAccum += dt;
            if (prevPosAccum > 0.2)
            {
                prevPosAccum = 0;
                prevPrevPos.Set(prevPos);
                prevPos.Set(entity.ServerPos.X, entity.ServerPos.Y, entity.ServerPos.Z);
            }

            // Long duration tests to make sure we're not just wobbling around in the same spot
            distCheckAccum += dt;
            if (distCheckAccum > 2)
            {
                distCheckAccum = 0;
                if (sqDistToTarget - lastDistToTarget < 0.1)
                {
                    stuck         = true;
                    stuckCounter += 30;
                }
                else if (!stuck)
                {
                    stuckCounter = 0;                 // Only reset the stuckCounter in same tick as doing this test; otherwise the stuckCounter gets set to 0 every 2 or 3 ticks even if the entity collided horizontally (because motion vecs get set to 0 after the collision, so won't collide in the successive tick)
                }
                lastDistToTarget = sqDistToTarget;
            }

            if (stuck)
            {
                stuckCounter++;
            }

            if (GlobalConstants.OverallSpeedMultiplier > 0 && stuckCounter > 40 / GlobalConstants.OverallSpeedMultiplier)
            {
                //entity.World.SpawnParticles(10, ColorUtil.WhiteArgb, prevPos, prevPos, new Vec3f(0, 0, 0), new Vec3f(0, -1, 0), 1, 1);
                Stop();
                OnStuck?.Invoke();
                return;
            }

            EntityControls controls = entity.MountedOn == null ? entity.Controls : entity.MountedOn.Controls;

            if (controls == null)
            {
                return;
            }

            targetVec.Set(
                (float)(target.X - entity.ServerPos.X),
                (float)(target.Y - entity.ServerPos.Y),
                (float)(target.Z - entity.ServerPos.Z)
                );
            targetVec.Normalize();

            float desiredYaw = 0;

            if (sqDistToTarget >= 0.01)
            {
                desiredYaw = (float)Math.Atan2(targetVec.X, targetVec.Z);
            }

            float nowMoveSpeed = movingSpeed;

            if (sqDistToTarget < 1)
            {
                nowMoveSpeed = Math.Max(0.005f, movingSpeed * Math.Max(sqDistToTarget, 0.2f));
            }

            float yawDist   = GameMath.AngleRadDistance(entity.ServerPos.Yaw, desiredYaw);
            float turnSpeed = curTurnRadPerSec * dt * GlobalConstants.OverallSpeedMultiplier * movingSpeed;

            entity.ServerPos.Yaw += GameMath.Clamp(yawDist, -turnSpeed, turnSpeed);
            entity.ServerPos.Yaw  = entity.ServerPos.Yaw % GameMath.TWOPI;



            double cosYaw = Math.Cos(entity.ServerPos.Yaw);
            double sinYaw = Math.Sin(entity.ServerPos.Yaw);

            controls.WalkVector.Set(sinYaw, GameMath.Clamp(targetVec.Y, -1, 1), cosYaw);
            controls.WalkVector.Mul(nowMoveSpeed * GlobalConstants.OverallSpeedMultiplier);

            // Make it walk along the wall, but not walk into the wall, which causes it to climb
            if (entity.Properties.RotateModelOnClimb && entity.Controls.IsClimbing && entity.ClimbingIntoFace != null && entity.Alive)
            {
                BlockFacing facing = entity.ClimbingIntoFace;
                if (Math.Sign(facing.Normali.X) == Math.Sign(controls.WalkVector.X))
                {
                    controls.WalkVector.X = 0;
                }

                if (Math.Sign(facing.Normali.Y) == Math.Sign(controls.WalkVector.Y))
                {
                    controls.WalkVector.Y = -controls.WalkVector.Y;
                }

                if (Math.Sign(facing.Normali.Z) == Math.Sign(controls.WalkVector.Z))
                {
                    controls.WalkVector.Z = 0;
                }
            }

            //   entity.World.SpawnParticles(0.3f, ColorUtil.WhiteAhsl, target, target, new Vec3f(), new Vec3f(), 0.1f, 0.1f, 3f, EnumParticleModel.Cube);

            if (entity.Properties.Habitat == EnumHabitat.Underwater)
            {
                controls.FlyVector.Set(controls.WalkVector);

                Vec3d pos                 = entity.Pos.XYZ;
                Block inblock             = entity.World.BlockAccessor.GetLiquidBlock((int)pos.X, (int)(pos.Y), (int)pos.Z);
                Block aboveblock          = entity.World.BlockAccessor.GetLiquidBlock((int)pos.X, (int)(pos.Y + 1), (int)pos.Z);
                float waterY              = (int)pos.Y + inblock.LiquidLevel / 8f + (aboveblock.IsLiquid() ? 9 / 8f : 0);
                float bottomSubmergedness = waterY - (float)pos.Y;

                // 0 = at swim line  1 = completely submerged
                float swimlineSubmergedness = GameMath.Clamp(bottomSubmergedness - ((float)entity.SwimmingOffsetY), 0, 1);
                swimlineSubmergedness = 1f - Math.Min(1f, swimlineSubmergedness + 0.5f);
                if (swimlineSubmergedness > 0f)
                {
                    //Push the fish back underwater if part is poking out ...  (may need future adaptation for sharks[?], probably by changing SwimmingOffsetY)
                    controls.FlyVector.Y = GameMath.Clamp(controls.FlyVector.Y, -0.04f, -0.02f) * (1f - swimlineSubmergedness);
                }
                else
                {
                    float factor = movingSpeed * GlobalConstants.OverallSpeedMultiplier / (float)Math.Sqrt(targetVec.X * targetVec.X + targetVec.Z * targetVec.Z);
                    controls.FlyVector.Y = targetVec.Y * factor;
                }

                if (entity.CollidedHorizontally)
                {
                    //TODO
                }
            }
            else if (entity.Swimming)
            {
                controls.FlyVector.Set(controls.WalkVector);

                Vec3d pos                 = entity.Pos.XYZ;
                Block inblock             = entity.World.BlockAccessor.GetLiquidBlock((int)pos.X, (int)(pos.Y), (int)pos.Z);
                Block aboveblock          = entity.World.BlockAccessor.GetLiquidBlock((int)pos.X, (int)(pos.Y + 1), (int)pos.Z);
                float waterY              = (int)pos.Y + inblock.LiquidLevel / 8f + (aboveblock.IsLiquid() ? 9 / 8f : 0);
                float bottomSubmergedness = waterY - (float)pos.Y;

                // 0 = at swim line
                // 1 = completely submerged
                float swimlineSubmergedness = GameMath.Clamp(bottomSubmergedness - ((float)entity.SwimmingOffsetY), 0, 1);
                swimlineSubmergedness = Math.Min(1, swimlineSubmergedness + 0.5f);
                controls.FlyVector.Y  = GameMath.Clamp(controls.FlyVector.Y, 0.02f, 0.04f) * swimlineSubmergedness;


                if (entity.CollidedHorizontally)
                {
                    controls.FlyVector.Y = 0.05f;
                }
            }
        }