// Convert an AnatomyEngine mesh to a Godot mesh:
public GodotMeshConverter(UVMesh mesh)
{
// Initialize the arrays that are needed:
Godot.Collections.Array arrays = new Godot.Collections.Array();
arrays.Resize((int)Mesh.ArrayType.Max);
int vertexCount = mesh.VertexList.Count;
Godot.Vector3[] normal_array = new Godot.Vector3[vertexCount];
Godot.Vector3[] vertex_array = new Godot.Vector3[vertexCount];
// Populate the arrays from the input mesh data:
for (int i = 0; i < vertexCount; i++)
{
Numerics.Vector3 vertex = mesh.VertexList[i].Position;
Numerics.Vector3 normal = mesh.VertexList[i].Normal;
vertex_array[i] = new Godot.Vector3(vertex.X, vertex.Y, vertex.Z);
normal_array[i] = new Godot.Vector3(normal.X, normal.Y, normal.Z);
}
// The index list doesn't need to be converted:
int[] index_array = mesh.IndexList.ToArray();
// Put the data arrays in a larger array for Godot to understand what the arrays represent:
arrays[(int)Mesh.ArrayType.Vertex] = vertex_array;
arrays[(int)Mesh.ArrayType.Normal] = normal_array;
arrays[(int)Mesh.ArrayType.Index] = index_array;
// Finally, upload the mesh to GPU:
this.AddSurfaceFromArrays(Mesh.PrimitiveType.Triangles, arrays);
}
/// <summary>
/// Converts world coordinate to cloud relative (top left) coordinates
/// </summary>
public void ConvertToCloudLocal(Vector3 worldPosition, out int x, out int y)
{
var topLeftRelative = worldPosition - Translation;
// Floor is used here because otherwise the last coordinate is wrong
x = ((int)Math.Floor((topLeftRelative.x + Constants.CLOUD_WIDTH) / Resolution)
+ position.x * Size / Constants.CLOUD_SQUARES_PER_SIDE) % Size;
y = ((int)Math.Floor((topLeftRelative.z + Constants.CLOUD_HEIGHT) / Resolution)
+ position.y * Size / Constants.CLOUD_SQUARES_PER_SIDE) % Size;
}
/// <summary>
/// Returns true if position with radius around it contains any
/// points that are within this cloud.
/// </summary>
public bool ContainsPositionWithRadius(Vector3 worldPosition,
float radius)
{
if (worldPosition.x + radius < Translation.x - Constants.CLOUD_WIDTH ||
worldPosition.x - radius >= Translation.x + Constants.CLOUD_WIDTH ||
worldPosition.z + radius < Translation.z - Constants.CLOUD_HEIGHT ||
worldPosition.z - radius >= Translation.z + Constants.CLOUD_HEIGHT)
{
return(false);
}
return(true);
}
public OneActorUpdate(Guid id, Godot.Vector3 localPos, Godot.Quat localQuat,
Godot.Vector3 appPos, Godot.Quat appQuat)
{
localTransform = new TransformPatch();
appTransform = new TransformPatch();
localTransform.Position = new Vector3Patch(localPos);
localTransform.Rotation = new QuaternionPatch(localQuat);
appTransform.Position = new Vector3Patch(appPos);
appTransform.Rotation = new QuaternionPatch(appQuat);
actorGuid = id;
}
public void SetVelocitiesFromTransformsDiff(float DT, RigidBodyTransform current,
RigidBodyTransform prev)
{
if (Math.Abs(DT) > 0.0005f)
{
float invUpdateDT = 1.0f / (DT); // DT has to be different from zero
Godot.Vector3 eulerAngles = (prev.Rotation.Inverse() * current.Rotation).GetEuler();
Godot.Vector3 radianAngles = UtilMethods.TransformEulerAnglesToRadians(eulerAngles);
//if (radianAngles.sqrMagnitude < 0.02F)
//{
// Debug.Log("NULL DT=" + DT + " prevRot=" + prev.Rotation.ToString() + " next=" + current.Rotation.ToString());
//}
AngularVelocity = radianAngles * invUpdateDT;
LinearVelocity = (current.Position - prev.Position) * invUpdateDT;
}
}
void MovePlayer()
{
velocity = Vector3.Zero;
if (Input.IsActionPressed("ui_forward"))
{
velocity += -Transform.basis.z * moveForce * deltaTime;
}
if (Input.IsActionPressed("ui_back"))
{
velocity += Transform.basis.z * moveForce * deltaTime;
}
if (Input.IsActionPressed("ui_up"))
{
velocity += Transform.basis.y * moveForce * deltaTime;
}
if (Input.IsActionPressed("ui_down"))
{
velocity += -Transform.basis.y * moveForce * deltaTime;
}
if (Input.IsActionPressed("ui_left"))
{
velocity += -Transform.basis.x * moveForce * deltaTime;
}
if (Input.IsActionPressed("ui_right"))
{
velocity += Transform.basis.x * moveForce * deltaTime;
}
if (Input.IsActionPressed("ui_speed"))
{
velocity += velocity * 2;
}
MoveAndCollide(velocity);
position = Translation;
}
/// <summary>
/// Checks if position is in this cloud, also returns relative coordinates
/// </summary>
public bool ContainsPosition(Vector3 worldPosition, out int x, out int y)
{
ConvertToCloudLocal(worldPosition, out x, out y);
return(x >= 0 && y >= 0 && x < Constants.CLOUD_WIDTH && y < Constants.CLOUD_HEIGHT);
}
/// <summary>
/// Generate rigid body transform snapshot for owned transforms with specified timestamp.
/// </summary>
/// <param name="time">Snapshot timestamp.</param>
/// <param name="root">Root Scene.</param>
/// <returns>Generated snapshot.</returns>
public Snapshot GenerateSnapshot(float time, Spatial root)
{
// collect transforms from owned rigid bodies
// and generate update packet/snapshot
// these constants define when a body is considered to be sleeping
const float globalToleranceMultipier = 1.0F;
const float maxSleepingSqrtLinearVelocity = 0.1F * globalToleranceMultipier;
const float maxSleepingSqrtAngularVelocity = 0.1F * globalToleranceMultipier;
const float maxSleepingSqrtPositionDiff = 0.02F * globalToleranceMultipier;
const float maxSleepingSqrtAngularEulerDiff = 0.15F * globalToleranceMultipier;
const short limitNoUpdateForSleepingBodies = 500;
const short numConsecutiveSleepingTrueConditionForNoUpdate = 5;
int numSleepingBodies = 0;
int numOwnedBodies = 0;
List <Snapshot.TransformInfo> transforms = new List <Snapshot.TransformInfo>(_rigidBodies.Count);
foreach (var rb in _rigidBodies.Values)
{
if (!rb.Ownership)
{
rb.numOfConsequentSleepingFrames = 0;
continue;
}
RigidBodyTransform transform;
{
transform.Position = root.ToLocal(rb.RigidBody.GlobalTransform.origin);
transform.Rotation = (root.GlobalTransform.basis.Inverse() * rb.RigidBody.GlobalTransform.basis).RotationQuat();
}
numOwnedBodies++;
Patching.Types.MotionType mType = (rb.IsKeyframed) ? (Patching.Types.MotionType.Keyframed)
: (Patching.Types.MotionType.Dynamic);
Godot.Vector3 posDiff = rb.lastValidLinerVelocityOrPos - transform.Position;
Godot.Vector3 rotDiff = UtilMethods.TransformEulerAnglesToRadians(rb.lastValidAngularVelocityorAng - transform.Rotation.GetEuler());
bool isBodySleepingInThisFrame =
(!rb.IsKeyframed) && // if body is key framed and owned then we should just feed the jitter buffer
(rb.RigidBody.LinearVelocity.LengthSquared() < maxSleepingSqrtLinearVelocity &&
rb.RigidBody.AngularVelocity.LengthSquared() < maxSleepingSqrtAngularVelocity &&
posDiff.LengthSquared() < maxSleepingSqrtPositionDiff &&
rotDiff.LengthSquared() < maxSleepingSqrtAngularEulerDiff);
if (isBodySleepingInThisFrame)
{
rb.numOfConsequentSleepingFrames++;
rb.numOfConsequentSleepingFrames = (rb.numOfConsequentSleepingFrames > (short)limitNoUpdateForSleepingBodies) ?
(short)limitNoUpdateForSleepingBodies : rb.numOfConsequentSleepingFrames;
}
else
{
rb.numOfConsequentSleepingFrames = 0;
}
// this is the real condition to put a body to sleep
bool isBodySleeping = (rb.numOfConsequentSleepingFrames > numConsecutiveSleepingTrueConditionForNoUpdate);
// test if this is sleeping, and when this was newly added then
if (rb.lastTimeKeyFramedUpdate > 0.001F && isBodySleeping &&
rb.sendMotionType == Patching.Types.MotionType.Sleeping &&
rb.numOfConsequentSleepingFrames < limitNoUpdateForSleepingBodies)
{
// condition for velocity and positions are triggered and we already told the consumers to make body sleep, so just skip this update
numSleepingBodies++;
continue;
}
mType = (isBodySleeping) ? (Patching.Types.MotionType.Sleeping) : mType;
// here we handle the case when after of 300 frames with no update we should send one update at least
if (rb.numOfConsequentSleepingFrames >= limitNoUpdateForSleepingBodies)
{
rb.numOfConsequentSleepingFrames = numConsecutiveSleepingTrueConditionForNoUpdate + 1;
}
// store the last update informations
rb.sendMotionType = mType;
rb.lastTimeKeyFramedUpdate = time;
rb.lastValidLinerVelocityOrPos = transform.Position;
rb.lastValidAngularVelocityorAng = transform.Rotation.GetEuler();
transforms.Add(new Snapshot.TransformInfo(rb.Id, transform, mType));
#if MRE_PHYSICS_DEBUG
GD.Print(" SEND Remote body: " //+ rb.Id.ToString() + " OriginalRot:" + transform.Rotation
//+ " RigidBodyRot:" + rb.RigidBody.GlobalTransform.basis.RotationQuat()
+ " lv:" + rb.RigidBody.LinearVelocity //+ " av:" + rb.RigidBody.AngularVelocity
+ " posDiff:" + posDiff /* + " rotDiff:" + rotDiff + " isKeyF:" + rb.IsKeyframed*/);
#endif
}
Snapshot.SnapshotFlags snapshotFlag = Snapshot.SnapshotFlags.NoFlags;
bool allBodiesAreSleepingNew = numOwnedBodies == numSleepingBodies;
if (allBodiesAreSleepingNew != _allOwnedBodiesAreSleeping)
{
_allOwnedBodiesAreSleeping = allBodiesAreSleepingNew;
snapshotFlag = Snapshot.SnapshotFlags.ResetJitterBuffer;
}
_lastNumberOfTransformsToBeSent = numOwnedBodies;
var ret = new Snapshot(time, transforms, snapshotFlag);
#if MRE_PHYSICS_DEBUG
GD.Print(" Client:" + " Total number of sleeping bodies: " + numSleepingBodies + " total RBs" + _rigidBodies.Count
+ " num owned " + numOwnedBodies + " num sent transforms " + transforms.Count
+ " send:" + ret.DoSendThisSnapshot());
#endif
return(ret);
}
public void FixedUpdate(Spatial root)
{
// - physics rigid body management
// -set transforms/velocities for key framed bodies
// get all the prediction time infos in this struct
PredictionTimeParameters timeInfo = new PredictionTimeParameters(1f / Engine.IterationsPerSecond);
// start the predictor
_predictor.StartBodyPredicitonForNextFrame();
int index = 0;
MultiSourceCombinedSnapshot snapshot;
_snapshotManager.Step(timeInfo.DT, out snapshot);
_snapshotManager.UpdateDebugDisplay(root); // guarded by ifdef internally
foreach (var rb in _rigidBodies.Values)
{
// if the body is owned then we only set the kinematic flag for the physics
if (rb.Ownership)
{
if (rb.IsKeyframed)
{
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Kinematic)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Kinematic;
}
}
else
{
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Rigid)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Rigid;
}
}
continue;
}
// Find corresponding rigid body info.
// since both are sorted list this should hit without index=0 at the beginning
while (index < snapshot.RigidBodies.Count && rb.Id.CompareTo(snapshot.RigidBodies.Values[index].Id) > 0)
{
index++;
}
if (index < snapshot.RigidBodies.Count && rb.Id == snapshot.RigidBodies.Values[index].Id)
{
// todo: kick-in prediction if we are missing an update for this rigid body
//if (!snapshot.RigidBodies.Values[index].HasUpdate)
//{
// rb.RigidBody.isKinematic = false;
// continue;
//}
RigidBodyTransform transform = snapshot.RigidBodies.Values[index].Transform;
float timeOfSnapshot = snapshot.RigidBodies.Values[index].LocalTime;
// get the key framed stream, and compute implicit velocities
Godot.Vector3 keyFramedPos = root.ToGlobal(transform.Position);
Godot.Quat keyFramedOrientation = root.GlobalTransform.basis.RotationQuat() * transform.Rotation;
Godot.Vector3 JBLinearVelocity =
root.GlobalTransform.basis.GetEuler() * snapshot.RigidBodies.Values[index].LinearVelocity;
Godot.Vector3 JBAngularVelocity =
root.GlobalTransform.basis.GetEuler() * snapshot.RigidBodies.Values[index].AngularVelocity;
// if there is a really new update then also store the implicit velocity
if (rb.lastTimeKeyFramedUpdate < timeOfSnapshot)
{
// we moved the velocity estimation into the jitter buffer
rb.lastValidLinerVelocityOrPos = JBLinearVelocity;
rb.lastValidAngularVelocityorAng = JBAngularVelocity;
#if MRE_PHYSICS_DEBUG
// test the source of large velocities
if (rb.lastValidLinerVelocityOrPos.LengthSquared() > _maxEstimatedLinearVelocity * _maxEstimatedLinearVelocity)
{
// limited debug version
GD.Print(" ACTIVE SPEED LIMIT TRAP RB: " //+ rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
+ " time:" + timeOfSnapshot
+ " newR:" + rb.lastTimeKeyFramedUpdate
+ " hasupdate:" + snapshot.RigidBodies.Values[index].HasUpdate);
//+ " DangE:" + eulerAngles + " DangR:" + radianAngles );
}
#endif
// cap the velocities
rb.lastValidLinerVelocityOrPos = ClampLength(
rb.lastValidLinerVelocityOrPos, _maxEstimatedLinearVelocity);
rb.lastValidAngularVelocityorAng = ClampLength(
rb.lastValidAngularVelocityorAng, _maxEstimatedAngularVelocity);
// if body is sleeping then all velocities are zero
if (snapshot.RigidBodies.Values[index].motionType == Patching.Types.MotionType.Sleeping)
{
rb.lastValidLinerVelocityOrPos = new Vector3(0.0F, 0.0F, 0.0F);
rb.lastValidAngularVelocityorAng = new Vector3(0.0F, 0.0F, 0.0F);
}
#if MRE_PHYSICS_DEBUG
if (true)
{
// limited debug version
GD.Print(" Remote body: " + rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
+ " time:" + timeOfSnapshot + " newR:" + rb.lastTimeKeyFramedUpdate);
}
else
{
GD.Print(" Remote body: " + rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
//+ " DangE:" + eulerAngles + " DangR:" + radianAngles
+ " time:" + timeOfSnapshot + " newp:" + keyFramedPos
+ " newR:" + keyFramedOrientation
+ " oldP:" + rb.RigidBody.GlobalTransform.origin
+ " oldR:" + rb.RigidBody.GlobalTransform.basis.RotationQuat()
+ " OriginalRot:" + transform.Rotation
//+ " keyF:" + rb.RigidBody.isKinematic
+ " KF:" + rb.IsKeyframed);
}
#endif
// cap the velocities
rb.lastValidLinerVelocityOrPos = ClampLength(
rb.lastValidLinerVelocityOrPos, _maxEstimatedLinearVelocity);
rb.lastValidAngularVelocityorAng = ClampLength(
rb.lastValidAngularVelocityorAng, _maxEstimatedAngularVelocity);
// if body is sleeping then all velocities are zero
if (snapshot.RigidBodies.Values[index].motionType == Patching.Types.MotionType.Sleeping)
{
rb.lastValidLinerVelocityOrPos = new Vector3(0.0F, 0.0F, 0.0F);
rb.lastValidAngularVelocityorAng = new Vector3(0.0F, 0.0F, 0.0F);
}
#if MRE_PHYSICS_DEBUG
GD.Print(" Remote body: " + rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
//+ " DangE:" + eulerAngles + " DangR:" + radianAngles
+ " time:" + timeOfSnapshot + " newp:" + keyFramedPos
+ " newR:" + keyFramedOrientation
+ " incUpdateDt:" + timeInfo.DT
+ " oldP:" + rb.RigidBody.GlobalTransform.origin
+ " oldR:" + rb.RigidBody.GlobalTransform.basis.RotationQuat()
+ " OriginalRot:" + transform.Rotation
//+ " keyF:" + rb.RigidBody.isKinematic
+ " KF:" + rb.IsKeyframed);
#endif
}
rb.lastTimeKeyFramedUpdate = timeOfSnapshot;
rb.IsKeyframed = (snapshot.RigidBodies.Values[index].motionType == Patching.Types.MotionType.Keyframed);
// code to disable prediction and to use just key framing (and comment out the prediction)
//rb.RigidBody.isKinematic = true;
//rb.RigidBody.transform.position = keyFramedPos;
//rb.RigidBody.transform.rotation = keyFramedOrientation;
//rb.RigidBody.velocity.Set(0.0f, 0.0f, 0.0f);
//rb.RigidBody.angularVelocity.Set(0.0f, 0.0f, 0.0f);
// call the predictor with this remotely owned body
_predictor.AddAndProcessRemoteBodyForPrediction(rb, transform,
keyFramedPos, keyFramedOrientation, timeOfSnapshot, timeInfo);
}
}
// call the predictor
_predictor.PredictAllRemoteBodiesWithOwnedBodies(ref _rigidBodies, timeInfo);
}
public void AddAndProcessRemoteBodyForPrediction(RigidBodyPhysicsBridgeInfo rb,
RigidBodyTransform transform, Godot.Vector3 keyFramedPos,
Godot.Quat keyFramedOrientation, float timeOfSnapshot,
PredictionTimeParameters timeInfo)
{
var collisionInfo = new CollisionSwitchInfo();
collisionInfo.startPosition = rb.RigidBody.GlobalTransform.origin;
collisionInfo.startOrientation = rb.RigidBody.GlobalTransform.basis.RotationQuat();
collisionInfo.rigidBodyId = rb.Id;
collisionInfo.isKeyframed = rb.IsKeyframed;
// test is this remote body is in the monitor stream or if this is grabbed & key framed then this should not be dynamic
if (_monitorCollisionInfo.ContainsKey(rb.Id) && !rb.IsKeyframed)
{
// dynamic
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Rigid)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Rigid;
}
collisionInfo.monitorInfo = _monitorCollisionInfo[rb.Id];
collisionInfo.monitorInfo.timeFromStartCollision += timeInfo.DT;
collisionInfo.linearVelocity = rb.RigidBody.LinearVelocity;
collisionInfo.angularVelocity = rb.RigidBody.AngularVelocity;
#if MRE_PHYSICS_DEBUG
GD.Print(" Remote body: " + rb.Id.ToString() + " is dynamic since:"
+ collisionInfo.monitorInfo.timeFromStartCollision + " relative distance:" + collisionInfo.monitorInfo.relativeDistance
+ " interpolation:" + collisionInfo.monitorInfo.keyframedInterpolationRatio);
#endif
// if time passes by then make a transformation between key framed and dynamic
// but only change the positions not the velocity
if (collisionInfo.monitorInfo.keyframedInterpolationRatio > 0.05f)
{
// interpolate between key framed and dynamic transforms
float t = collisionInfo.monitorInfo.keyframedInterpolationRatio;
Godot.Vector3 interpolatedPos;
Godot.Quat interpolatedQuad;
interpolatedPos = t * keyFramedPos + (1.0f - t) * rb.RigidBody.GlobalTransform.origin;
interpolatedQuad = keyFramedOrientation.Slerp(rb.RigidBody.GlobalTransform.basis.RotationQuat(), t);
#if MRE_PHYSICS_DEBUG
GD.Print(" Interpolate body " + rb.Id.ToString() + " t=" + t
+ " time=" + OS.GetTicksMsec() * 0.001
+ " pos KF:" + keyFramedPos
+ " dyn:" + rb.RigidBody.Transform.origin
+ " interp pos:" + interpolatedPos
+ " rb vel:" + rb.RigidBody.LinearVelocity
+ " KF vel:" + rb.lastValidLinerVelocityOrPos);
#endif
// apply these changes only if they are significant in order to not to bother the physics engine
// for settled objects
Godot.Vector3 posdiff = rb.RigidBody.GlobalTransform.origin - interpolatedPos;
Vector3 rigidBodyOrigin = rb.RigidBody.GlobalTransform.origin;
Basis rigidBodyBasis = rb.RigidBody.GlobalTransform.basis;
if (posdiff.Length() > interpolationPosEpsilon)
{
rigidBodyOrigin = interpolatedPos;
}
float angleDiff = Math.Abs(Mathf.Rad2Deg(rigidBodyBasis.GetEuler().AngleTo(interpolatedQuad.GetEuler())));
if (angleDiff > interpolationAngularEps)
{
rigidBodyBasis = new Basis(interpolatedQuad);
}
rb.RigidBody.GlobalTransform = new Transform(rigidBodyBasis, rigidBodyOrigin);
// apply velocity damping if we are in the interpolation phase
if (collisionInfo.monitorInfo.keyframedInterpolationRatio >= velocityDampingInterpolationValueStart)
{
rb.RigidBody.LinearVelocity *= velocityDampingForInterpolation;
rb.RigidBody.AngularVelocity *= velocityDampingForInterpolation;
}
}
}
else
{
// 100% key framing
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Kinematic)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Kinematic;
}
rb.RigidBody.GlobalTransform = new Transform(new Basis(keyFramedOrientation), keyFramedPos);
rb.RigidBody.LinearVelocity = new Vector3(0.0f, 0.0f, 0.0f);
rb.RigidBody.AngularVelocity = new Vector3(0.0f, 0.0f, 0.0f);
collisionInfo.linearVelocity = rb.lastValidLinerVelocityOrPos;
collisionInfo.angularVelocity = rb.lastValidAngularVelocityorAng;
collisionInfo.monitorInfo = new CollisionMonitorInfo();
#if MRE_PHYSICS_DEBUG
if (rb.IsKeyframed)
{
GD.Print(" Remote body: " + rb.Id.ToString() + " is key framed:"
+ " linvel:" + collisionInfo.linearVelocity
+ " angvel:" + collisionInfo.angularVelocity);
}
#endif
}
// <todo> add more filtering here to cancel out unnecessary items,
// but for small number of bodies should be OK
_switchCollisionInfos.Add(collisionInfo);
}
public static float Distance(Godot.Vector3 from, Godot.Vector3 to)
{
return(Vector3.Distance(new Vector3(from.x, from.y, from.z), new Vector3(to.x, to.y, to.z)));
}
public static Vector3 FromV3(Godot.Vector3 godotV)
{
return(new Vector3(godotV.x, godotV.y, godotV.z));
}
