Miko/libremetaverse
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
eb8ee37b34f89d759b57a05d698b0e71bd55ecdf
libremetaverse/Programs/Simian/Extensions/Movement.cs

449 lines
22 KiB
C#
Raw Blame History

using OpenMetaverse;
using OpenMetaverse.Packets;
using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;
namespace Simian.Extensions
{
public class Movement : ISimianExtension
{
const int UPDATE_ITERATION = 100; //rate in milliseconds to send ObjectUpdate
const bool ENVIRONMENT_SOUNDS = true; //collision sounds, splashing, etc
const float GRAVITY = 9.8f; //meters/sec
const float WALK_SPEED = 3f; //meters/sec
const float RUN_SPEED = 5f; //meters/sec
const float FLY_SPEED = 10f; //meters/sec
const float FALL_DELAY = 0.33f; //seconds before starting animation
const float FALL_FORGIVENESS = 0.25f; //fall buffer in meters
const float JUMP_IMPULSE_VERTICAL = 8.5f; //boost amount in meters/sec
const float JUMP_IMPULSE_HORIZONTAL = 10f; //boost amount in meters/sec (no clue why this is so high)
const float PREJUMP_DELAY = 0.25f; //seconds before actually jumping
const float AVATAR_TERMINAL_VELOCITY = 54f; //~120mph
const float SQRT_TWO = 1.41421356f;
Simian server;
Timer updateTimer;
long lastTick;
public int LastTick
{
get { return (int) Interlocked.Read(ref lastTick); }
set { Interlocked.Exchange(ref lastTick, value); }
}
public Movement(Simian server)
{
this.server = server;
}
public void Start()
{
server.UDPServer.RegisterPacketCallback(PacketType.AgentUpdate, new UDPServer.PacketCallback(AgentUpdateHandler));
server.UDPServer.RegisterPacketCallback(PacketType.AgentHeightWidth, new UDPServer.PacketCallback(AgentHeightWidthHandler));
server.UDPServer.RegisterPacketCallback(PacketType.SetAlwaysRun, new UDPServer.PacketCallback(SetAlwaysRunHandler));
updateTimer = new Timer(new TimerCallback(UpdateTimer_Elapsed));
LastTick = Environment.TickCount;
updateTimer.Change(UPDATE_ITERATION, UPDATE_ITERATION);
}
public void Stop()
{
updateTimer.Dispose();
}
void UpdateTimer_Elapsed(object sender)
{
int tick = Environment.TickCount;
float seconds = (float)((tick - LastTick) / 1000f);
LastTick = tick;
lock (server.Agents)
{
foreach (Agent agent in server.Agents.Values)
{
bool animsChanged = false;
// Create forward and left vectors from the current avatar rotation
Matrix4 rotMatrix = Matrix4.CreateFromQuaternion(agent.Avatar.Rotation);
Vector3 fwd = Vector3.Transform(Vector3.UnitX, rotMatrix);
Vector3 left = Vector3.Transform(Vector3.UnitY, rotMatrix);
// Check control flags
bool heldForward = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_AT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_AT_POS;
bool heldBack = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_AT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_AT_NEG;
bool heldLeft = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_LEFT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_LEFT_POS;
bool heldRight = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG;
bool heldTurnLeft = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_LEFT) == AgentManager.ControlFlags.AGENT_CONTROL_TURN_LEFT;
bool heldTurnRight = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_RIGHT) == AgentManager.ControlFlags.AGENT_CONTROL_TURN_RIGHT;
bool heldUp = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) == AgentManager.ControlFlags.AGENT_CONTROL_UP_POS;
bool heldDown = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG;
bool flying = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_FLY) == AgentManager.ControlFlags.AGENT_CONTROL_FLY;
bool mouselook = (agent.ControlFlags & AgentManager.ControlFlags.AGENT_CONTROL_MOUSELOOK) == AgentManager.ControlFlags.AGENT_CONTROL_MOUSELOOK;
// direction in which the avatar is trying to move
Vector3 move = Vector3.Zero;
if (heldForward) { move.X += fwd.X; move.Y += fwd.Y; }
if (heldBack) { move.X -= fwd.X; move.Y -= fwd.Y; }
if (heldLeft) { move.X += left.X; move.Y += left.Y; }
if (heldRight) { move.X -= left.X; move.Y -= left.Y; }
if (heldUp) { move.Z += 1; }
if (heldDown) { move.Z -= 1; }
// is the avatar trying to move?
bool moving = move != Vector3.Zero;
bool jumping = agent.TickJump > 0;
// 2-dimensional speed multipler
float speed = seconds * (flying ? FLY_SPEED : agent.Running && !jumping ? RUN_SPEED : WALK_SPEED);
if ((heldForward || heldBack) && (heldLeft || heldRight))
speed /= SQRT_TWO;
// adjust multiplier for Z dimension
float oldFloor = GetLandHeightAt(agent.Avatar.Position);
float newFloor = GetLandHeightAt(agent.Avatar.Position + (move * speed));
if (!flying && newFloor != oldFloor)
speed /= (1 + (SQRT_TWO * Math.Abs(newFloor - oldFloor)));
// least possible distance from avatar to the ground
// TODO: calculate to get rid of "bot squat"
float lowerLimit = newFloor + agent.Avatar.Scale.Z / 2;
// Z acceleration resulting from gravity
float gravity = 0f;
float waterChestHeight = server.WaterHeight - (agent.Avatar.Scale.Z * .33f);
if (flying)
{
agent.TickFall = 0;
agent.TickJump = 0;
//velocity falloff while flying
agent.Avatar.Velocity.X *= 0.66f;
agent.Avatar.Velocity.Y *= 0.66f;
agent.Avatar.Velocity.Z *= 0.33f;
if (move.X != 0 || move.Y != 0)
{ //flying horizontally
if (server.Avatars.SetDefaultAnimation(agent, Animations.FLY))
animsChanged = true;
}
else if (move.Z > 0)
{ //flying straight up
if (server.Avatars.SetDefaultAnimation(agent, Animations.HOVER_UP))
animsChanged = true;
}
else if (move.Z < 0)
{ //flying straight down
if (server.Avatars.SetDefaultAnimation(agent, Animations.HOVER_DOWN))
animsChanged = true;
}
else
{ //hovering in the air
if (server.Avatars.SetDefaultAnimation(agent, Animations.HOVER))
animsChanged = true;
}
}
else if (agent.Avatar.Position.Z > lowerLimit + FALL_FORGIVENESS || agent.Avatar.Position.Z <= waterChestHeight)
{ //falling or landing from a jump
if (agent.Avatar.Position.Z > server.WaterHeight)
{ //above water
move = Vector3.Zero; //override controls while drifting
agent.Avatar.Velocity *= 0.95f; //keep most of our inertia
float fallElapsed = (float)(Environment.TickCount - agent.TickFall) / 1000f;
if (agent.TickFall == 0 || (fallElapsed > FALL_DELAY && agent.Avatar.Velocity.Z >= 0f))
{ //just started falling
agent.TickFall = Environment.TickCount;
}
else
{
gravity = GRAVITY * fallElapsed * seconds; //normal gravity
if (!jumping)
{ //falling
if (fallElapsed > FALL_DELAY)
{ //falling long enough to trigger the animation
if (server.Avatars.SetDefaultAnimation(agent, Animations.FALLDOWN))
animsChanged = true;
}
}
}
}
else if (agent.Avatar.Position.Z >= waterChestHeight)
{ //at the water line
gravity = 0f;
agent.Avatar.Velocity *= 0.5f;
agent.Avatar.Velocity.Z = 0f;
if (move.Z < 1) agent.Avatar.Position.Z = waterChestHeight;
if (move.Z > 0)
{
if (server.Avatars.SetDefaultAnimation(agent, Animations.HOVER_UP))
animsChanged = true;
}
else if (move.X != 0 || move.Y != 0)
{
if (server.Avatars.SetDefaultAnimation(agent, Animations.FLYSLOW))
animsChanged = true;
}
else
{
if (server.Avatars.SetDefaultAnimation(agent, Animations.HOVER))
animsChanged = true;
}
}
else
{ //underwater
gravity = 0f; //buoyant
agent.Avatar.Velocity *= 0.5f * seconds;
agent.Avatar.Velocity.Z += 1.0f * seconds;
if (server.Avatars.SetDefaultAnimation(agent, Animations.FALLDOWN))
animsChanged = true;
}
}
else
{ //on the ground
agent.TickFall = 0;
//friction
agent.Avatar.Acceleration *= 0.2f;
agent.Avatar.Velocity *= 0.2f;
agent.Avatar.Position.Z = lowerLimit;
if (move.Z > 0)
{ //jumping
if (!jumping)
{ //begin prejump
move.Z = 0; //override Z control
if (server.Avatars.SetDefaultAnimation(agent, Animations.PRE_JUMP))
animsChanged = true;
agent.TickJump = Environment.TickCount;
}
else if (Environment.TickCount - agent.TickJump > PREJUMP_DELAY * 1000)
{ //start actual jump
if (server.Avatars.SetDefaultAnimation(agent, Animations.JUMP))
animsChanged = true;
agent.Avatar.Velocity.Z = JUMP_IMPULSE_VERTICAL * seconds;
agent.Avatar.Velocity.X += agent.Avatar.Acceleration.X * JUMP_IMPULSE_HORIZONTAL;
agent.Avatar.Velocity.Y += agent.Avatar.Acceleration.Y * JUMP_IMPULSE_HORIZONTAL;
}
else move.Z = 0; //override Z control
}
else
{ //not jumping
agent.TickJump = 0;
if (move.X != 0 || move.Y != 0)
{ //not walking
if (move.Z < 0)
{ //crouchwalking
if (server.Avatars.SetDefaultAnimation(agent, Animations.CROUCHWALK))
animsChanged = true;
}
else if (agent.Running)
{ //running
if (server.Avatars.SetDefaultAnimation(agent, Animations.RUN))
animsChanged = true;
}
else
{ //walking
if (server.Avatars.SetDefaultAnimation(agent, Animations.WALK))
animsChanged = true;
}
}
else
{ //walking
if (move.Z < 0)
{ //crouching
if (server.Avatars.SetDefaultAnimation(agent, Animations.CROUCH))
animsChanged = true;
}
else
{ //standing
if (server.Avatars.SetDefaultAnimation(agent, Animations.STAND))
animsChanged = true;
}
}
}
}
if (animsChanged)
server.Avatars.SendAnimations(agent);
// static acceleration when any control is held, otherwise none
if (moving) agent.Avatar.Acceleration = move * speed; //FIXME
else agent.Avatar.Acceleration = Vector3.Zero;
float maxVel = AVATAR_TERMINAL_VELOCITY * seconds;
if (gravity > maxVel) gravity = maxVel;
agent.Avatar.Velocity += agent.Avatar.Acceleration - new Vector3(0f, 0f, gravity);
agent.Avatar.Position += agent.Avatar.Velocity;
if (agent.Avatar.Position.X < 0) agent.Avatar.Position.X = 0f;
else if (agent.Avatar.Position.X > 255) agent.Avatar.Position.X = 255f;
if (agent.Avatar.Position.Y < 0) agent.Avatar.Position.Y = 0f;
else if (agent.Avatar.Position.Y > 255) agent.Avatar.Position.Y = 255f;
if (agent.Avatar.Position.Z < lowerLimit) agent.Avatar.Position.Z = lowerLimit;
}
}
}
void AgentUpdateHandler(Packet packet, Agent agent)
{
AgentUpdatePacket update = (AgentUpdatePacket)packet;
agent.Avatar.Rotation = update.AgentData.BodyRotation;
agent.ControlFlags = (AgentManager.ControlFlags)update.AgentData.ControlFlags;
agent.State = update.AgentData.State;
agent.Flags = (PrimFlags)update.AgentData.Flags;
ObjectUpdatePacket fullUpdate = BuildFullUpdate(agent.Avatar,
NameValue.NameValuesToString(agent.Avatar.NameValues), server.RegionHandle,
agent.State, agent.Flags);
lock (server.Agents)
{
foreach (Agent recipient in server.Agents.Values)
recipient.SendPacket(fullUpdate);
}
}
void SetAlwaysRunHandler(Packet packet, Agent agent)
{
SetAlwaysRunPacket run = (SetAlwaysRunPacket)packet;
agent.Running = run.AgentData.AlwaysRun;
}
float GetLandHeightAt(Vector3 position)
{
int x = (int)position.X;
int y = (int)position.Y;
if (x > 255) x = 255;
else if (x < 0) x = 0;
if (y > 255) y = 255;
else if (y < 0) y = 0;
float center = server.Heightmap[y * 256 + x];
float distX = position.X - (int)position.X;
float distY = position.Y - (int)position.Y;
float nearestX;
float nearestY;
if (distX > 0) nearestX = server.Heightmap[y * 256 + x + (x < 255 ? 1 : 0)];
else nearestX = server.Heightmap[y * 256 + x - (x > 0 ? 1 : 0)];
if (distY > 0) nearestY = server.Heightmap[(y + (y < 255 ? 1 : 0)) * 256 + x];
else nearestY = server.Heightmap[(y - (y > 0 ? 1 : 0)) * 256 + x];
float lerpX = Utils.Lerp(center, nearestX, Math.Abs(distX));
float lerpY = Utils.Lerp(center, nearestY, Math.Abs(distY));
return ((lerpX + lerpY) / 2);
}
void AgentHeightWidthHandler(Packet packet, Agent agent)
{
AgentHeightWidthPacket heightWidth = (AgentHeightWidthPacket)packet;
Logger.Log(String.Format("Agent wants to set height={0}, width={1}",
heightWidth.HeightWidthBlock.Height, heightWidth.HeightWidthBlock.Width), Helpers.LogLevel.Info);
}
public static ObjectUpdatePacket BuildFullUpdate(Primitive obj, string nameValues, ulong regionHandle,
byte state, PrimFlags flags)
{
byte[] objectData = new byte[60];
int pos = 0;
obj.Position.GetBytes().CopyTo(objectData, pos);
pos += 12;
obj.Velocity.GetBytes().CopyTo(objectData, pos);
pos += 12;
obj.Acceleration.GetBytes().CopyTo(objectData, pos);
pos += 12;
obj.Rotation.GetBytes().CopyTo(objectData, pos);
pos += 12;
obj.AngularVelocity.GetBytes().CopyTo(objectData, pos);
ObjectUpdatePacket update = new ObjectUpdatePacket();
update.RegionData.RegionHandle = regionHandle;
update.RegionData.TimeDilation = UInt16.MaxValue;
update.ObjectData = new ObjectUpdatePacket.ObjectDataBlock[1];
update.ObjectData[0] = new ObjectUpdatePacket.ObjectDataBlock();
update.ObjectData[0].ClickAction = (byte)obj.ClickAction;
update.ObjectData[0].CRC = 0;
update.ObjectData[0].ExtraParams = new byte[0]; //FIXME: Need a serializer for ExtraParams
update.ObjectData[0].Flags = (byte)flags;
update.ObjectData[0].FullID = obj.ID;
update.ObjectData[0].Gain = obj.SoundGain;
update.ObjectData[0].ID = obj.LocalID;
update.ObjectData[0].JointAxisOrAnchor = obj.JointAxisOrAnchor;
update.ObjectData[0].JointPivot = obj.JointPivot;
update.ObjectData[0].JointType = (byte)obj.Joint;
update.ObjectData[0].Material = (byte)obj.PrimData.Material;
update.ObjectData[0].MediaURL = new byte[0]; // FIXME:
update.ObjectData[0].NameValue = Utils.StringToBytes(nameValues);
update.ObjectData[0].ObjectData = objectData;
update.ObjectData[0].OwnerID = obj.Properties.OwnerID;
update.ObjectData[0].ParentID = obj.ParentID;
update.ObjectData[0].PathBegin = Primitive.PackBeginCut(obj.PrimData.PathBegin);
update.ObjectData[0].PathCurve = (byte)obj.PrimData.PathCurve;
update.ObjectData[0].PathEnd = Primitive.PackEndCut(obj.PrimData.PathEnd);
update.ObjectData[0].PathRadiusOffset = Primitive.PackPathTwist(obj.PrimData.PathRadiusOffset);
update.ObjectData[0].PathRevolutions = Primitive.PackPathRevolutions(obj.PrimData.PathRevolutions);
update.ObjectData[0].PathScaleX = Primitive.PackPathScale(obj.PrimData.PathScaleX);
update.ObjectData[0].PathScaleY = Primitive.PackPathScale(obj.PrimData.PathScaleY);
update.ObjectData[0].PathShearX = (byte)Primitive.PackPathShear(obj.PrimData.PathShearX);
update.ObjectData[0].PathShearY = (byte)Primitive.PackPathShear(obj.PrimData.PathShearY);
update.ObjectData[0].PathSkew = Primitive.PackPathTwist(obj.PrimData.PathSkew);
update.ObjectData[0].PathTaperX = Primitive.PackPathTaper(obj.PrimData.PathTaperX);
update.ObjectData[0].PathTaperY = Primitive.PackPathTaper(obj.PrimData.PathTaperY);
update.ObjectData[0].PathTwist = Primitive.PackPathTwist(obj.PrimData.PathTwist);
update.ObjectData[0].PathTwistBegin = Primitive.PackPathTwist(obj.PrimData.PathTwistBegin);
update.ObjectData[0].PCode = (byte)obj.PrimData.PCode;
update.ObjectData[0].ProfileBegin = Primitive.PackBeginCut(obj.PrimData.ProfileBegin);
update.ObjectData[0].ProfileCurve = (byte)obj.PrimData.ProfileCurve;
update.ObjectData[0].ProfileEnd = Primitive.PackEndCut(obj.PrimData.ProfileEnd);
update.ObjectData[0].ProfileHollow = Primitive.PackProfileHollow(obj.PrimData.ProfileHollow);
update.ObjectData[0].PSBlock = new byte[0]; // FIXME:
update.ObjectData[0].TextColor = obj.TextColor.GetBytes(true);
update.ObjectData[0].TextureAnim = obj.TextureAnim.GetBytes();
update.ObjectData[0].TextureEntry = obj.Textures.ToBytes();
update.ObjectData[0].Radius = obj.SoundRadius;
update.ObjectData[0].Scale = obj.Scale;
update.ObjectData[0].Sound = obj.Sound;
update.ObjectData[0].State = state;
update.ObjectData[0].Text = Utils.StringToBytes(obj.Text);
update.ObjectData[0].UpdateFlags = (uint)flags;
update.ObjectData[0].Data = new byte[0]; // FIXME:
return update;
}
}
}
Reference in New Issue View Git Blame Copy Permalink