void ContactDebug_OnDataReady(IEnumerable <KeyValuePair <PhysX.Actor, int> > data)
{
m_log.InfoFormat("[InWorldz.PhysX.Debugging] Contact Dump --");
foreach (var kvp in data)
{
if (kvp.Key.UserData == null)
{
continue;
}
PhysxPrim prim = kvp.Key.UserData as PhysxPrim;
if (prim == null)
{
m_log.DebugFormat("[InWorldz.PhysX.Debugging]: (object) {0}", kvp.Value);
}
else
{
OpenMetaverse.Vector3 pos = prim.Position;
m_log.DebugFormat("[InWorldz.PhysX.Debugging]: {0} {1} at {2}/{3}/{4}", prim.SOPName, kvp.Value,
(int)(pos.X + 0.5), (int)(pos.Y + 0.5), (int)(pos.Z + 0.5));
}
}
ContactDebug.OnDataReady -= new Debugging.ContactDebugManager.DataCallback(ContactDebug_OnDataReady);
}
public AddForceCmd(PhysicsActor actor, OpenMetaverse.Vector3 force, OpenMetaverse.Vector3 forceOffset, ForceType type)
{
Actor = actor;
Force = force;
Type = type;
ForceOffset = forceOffset;
}
public override void GatherTerseUpdate(out OpenMetaverse.Vector3 position, out OpenMetaverse.Quaternion rotation, out OpenMetaverse.Vector3 velocity, out OpenMetaverse.Vector3 acceleration, out OpenMetaverse.Vector3 angularVelocity)
{
lock (_terseConsistencyLock)
{
position = _position;
rotation = _rotation;
velocity = _velocity;
//if this is a child prim, or it is not physical, report angular velocity
//as whatever our target is (targetomega)
//this is necessary because changing this physical and back again will
//change the angular velocity and currently TargetOmega is implemented
//as an overwrite to angular velocity which is wrong.
if (IsChild || !_isPhysical)
{
angularVelocity = _properties.AngularVelocityTarget;
}
else if (_useAngularVelocity)
{
angularVelocity = _angularVelocity;
}
else
{
angularVelocity = OpenMetaverse.Vector3.Zero;
}
acceleration = _acceleration;
}
}
public override void LockAngularMotion(OpenMetaverse.Vector3 axis)
{
lock (_properties)
{
_properties.LockedAxes = axis;
}
}
public AddForceCmd(PhysicsActor actor, OpenMetaverse.Vector3 force, OpenMetaverse.Vector3 forceOffset, ForceType type)
{
Actor = actor;
Force = force;
Type = type;
ForceOffset = forceOffset;
}
public void StaticUpdated(uint actorID, OpenMetaverse.Vector3 position, OpenMetaverse.Quaternion orientation)
{
// Update the result variables
m_resultStaticID = actorID;
m_resultPosition = position;
m_resultOrientation = orientation;
m_staticResultsReceived = true;
}
public override void ForceAboveParcel(float height)
{
OpenMetaverse.Vector3 newPos = _position;
//place this object back above the parcel
_position.Z = height;
_position = newPos;
}
public static LSL_Vector EHArgUnwrapVector(object x)
{
if (x is OpenMetaverse.Vector3)
{
OpenMetaverse.Vector3 v = (OpenMetaverse.Vector3)x;
return(new LSL_Vector(v.X, v.Y, v.Z));
}
return((LSL_Vector)x);
}
public CreateCharacterCmd(float height, float radius, OpenMetaverse.Vector3 pos,
OpenMetaverse.Quaternion rot, bool flying, OpenMetaverse.Vector3 initialVelocity)
{
_height = height;
_radius = radius;
_position = pos;
_rotation = rot;
_flying = flying;
_initialVelocity = initialVelocity;
}
public override PhysicsActor AddAvatar(string avName, OpenMetaverse.Vector3 position, OpenMetaverse.Quaternion rotation, OpenMetaverse.Vector3 size, bool isFlying, OpenMetaverse.Vector3 initialVelocity)
{
Commands.CreateCharacterCmd cmd = new Commands.CreateCharacterCmd(size.Z, size.X, position, rotation, isFlying, initialVelocity);
this.QueueCommand(cmd);
cmd.FinshedEvent.Wait();
cmd.Dispose();
return(cmd.FinalActor);
}
public override void SetGrabSpinVelocity(OpenMetaverse.Vector3 target)
{
lock (_properties)
{
if (!_properties.BlockGrab)
{
_dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(target), PhysX.ForceMode.VelocityChange, true);
}
}
}
public override void SetMoveToTarget(OpenMetaverse.Vector3 target, float tau)
{
lock (_properties)
{
_properties.MoveTarget = target;
_properties.MoveTargetTau = tau;
}
_scene.QueueCommand(new Commands.WakeUpCmd(this));
}
public CreateCharacterCmd(float height, float radius, OpenMetaverse.Vector3 pos,
OpenMetaverse.Quaternion rot, bool flying, OpenMetaverse.Vector3 initialVelocity)
{
_height = height;
_radius = radius;
_position = pos;
_rotation = rot;
_flying = flying;
_initialVelocity = initialVelocity;
}
public override void UpdateOffsetPosition(OpenMetaverse.Vector3 newOffset, OpenMetaverse.Quaternion rotOffset)
{
//Offset updates are only handled for child prims. Upstream a non child move is
//handled by setting our position directly
if (IsChild)
{
_position = newOffset;
_rotation = rotOffset;
}
}
public override void LinkToNewParent(PhysicsActor obj, OpenMetaverse.Vector3 localPos, OpenMetaverse.Quaternion localRot)
{
lock (_terseConsistencyLock)
{
_position = localPos;
_rotation = localRot;
}
_parentPrim = (BasicPrim)obj;
}
public override void DelinkFromParent(OpenMetaverse.Vector3 newWorldPosition, OpenMetaverse.Quaternion newWorldRotation)
{
lock (_terseConsistencyLock)
{
_position = newWorldPosition;
_rotation = newWorldRotation;
}
_parentPrim = null;
}
public override void RayCastWorld(OpenMetaverse.Vector3 start, OpenMetaverse.Vector3 direction, float distance,
int hitAmounts, Action <List <ContactResult> > result)
{
QueueCommand(
new Commands.GenericSyncCmd(
(PhysxScene scene) =>
{
GetRayCastResults(start, direction, distance, hitAmounts, result, scene);
}
));
}
public static String AvatarMoved(string uuid, OpenMetaverse.Vector3 position, OpenMetaverse.Quaternion rotation, OpenMetaverse.Vector3 velocity)
{
Dictionary <string, object> msg = new Dictionary <string, object>();
msg ["type"] = "moveAvatar";
msg ["uuid"] = uuid;
msg ["position"] = position;
msg ["rotation"] = rotation;
msg ["velocity"] = velocity;
return(MGMJson.Encode(msg));
}
public void DynamicUpdated(uint actorID, OpenMetaverse.Vector3 position, OpenMetaverse.Quaternion orientation,
OpenMetaverse.Vector3 linearVelocity, OpenMetaverse.Vector3 angularVelocity)
{
Console.WriteLine("Received dynamic results!");
// Update the result variables
m_resultDynamicID = actorID;
m_resultPosition = position;
m_resultOrientation = orientation;
m_resultLinearVelocity = linearVelocity;
m_resultAngularVelocity = angularVelocity;
m_dynamicResultsReceived = true;
}
public ChangeChildPrimOffsetCmd(PhysxPrim parent, PhysxPrim child, OpenMetaverse.Vector3 newOffset,
OpenMetaverse.Quaternion rotOffset)
{
Util.ThrowIfNull(parent, "parent");
Util.ThrowIfNull(child, "child");
_parent = parent;
_child = child;
_newOffset = newOffset;
_rotOffset = rotOffset;
_affectedPrims = new List<PhysxPrim> { parent, child };
}
public UnlinkFromParentCmd(PhysxPrim child, PhysxPrim parent, OpenMetaverse.Vector3 newWorldPosition,
OpenMetaverse.Quaternion newWorldRotation)
{
Util.ThrowIfNull(parent, "parent");
Util.ThrowIfNull(child, "child");
_child = child;
_parent = parent;
_newWorldPosition = newWorldPosition;
_newWorldRotation = newWorldRotation;
_targetPrims = new PhysxPrim[] { child, parent };
}
public UnlinkFromParentCmd(PhysxPrim child, PhysxPrim parent, OpenMetaverse.Vector3 newWorldPosition,
OpenMetaverse.Quaternion newWorldRotation)
{
Util.ThrowIfNull(parent, "parent");
Util.ThrowIfNull(child, "child");
_child = child;
_parent = parent;
_newWorldPosition = newWorldPosition;
_newWorldRotation = newWorldRotation;
_targetPrims = new PhysxPrim[] { child, parent };
}
public ChangeChildPrimOffsetCmd(PhysxPrim parent, PhysxPrim child, OpenMetaverse.Vector3 newOffset,
OpenMetaverse.Quaternion rotOffset)
{
Util.ThrowIfNull(parent, "parent");
Util.ThrowIfNull(child, "child");
_parent = parent;
_child = child;
_newOffset = newOffset;
_rotOffset = rotOffset;
_affectedPrims = new List <PhysxPrim> {
parent, child
};
}
private void DoAxisLock(float timeStep, uint frameNum)
{
OpenMetaverse.Vector3 lockedaxis;
float gtau;
lock (_properties)
{
lockedaxis = _properties.LockedAxes;
gtau = _properties.GrabTargetTau;
}
// Grab overrides axis lock
if (gtau != 0)
{
return;
}
// Fast bypass: If all axes are unlocked, skip the math.
if (lockedaxis.X == 0 && lockedaxis.Y == 0 && lockedaxis.Z == 0)
{
return;
}
// Convert angular velocity to local.
OpenMetaverse.Vector3 localangvel = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity) * OpenMetaverse.Quaternion.Inverse(_rotation);
// Stop angular velocity on locked local axes (rotaxis.N == 0 means axis is locked)
if (lockedaxis.X != 0)
{
localangvel.X = 0;
}
if (lockedaxis.Y != 0)
{
localangvel.Y = 0;
}
if (lockedaxis.Z != 0)
{
localangvel.Z = 0;
}
// Convert to global angular velocity.
PhysX.Math.Vector3 angvel = PhysUtil.OmvVectorToPhysx(localangvel * _rotation);
// This is a harsh way to do this, but a locked axis must have no angular velocity whatsoever.
if (angvel != _dynActor.AngularVelocity)
{
_dynActor.ClearTorque();
_dynActor.AngularVelocity = angvel;
}
}
public PrepChildPrimAndLinkCmd(PhysxPrim parent, PhysxPrim child, OpenMetaverse.Vector3 localPos,
OpenMetaverse.Quaternion localRot)
{
Util.ThrowIfNull(parent, "parent");
Util.ThrowIfNull(child, "child");
_parent = parent;
_child = child;
_localPos = localPos;
_localRot = localRot;
_newShape = null;
_affectedPrims = new List<PhysxPrim> { parent, child };
_affectedPrims.AddRange(child.ChildShapes.Keys);
}
public override List <ContactResult> RayCastWorld(OpenMetaverse.Vector3 start, OpenMetaverse.Vector3 direction, float distance,
int hitAmounts)
{
List <ContactResult> contactResults = new List <ContactResult>();
AutoResetEvent ev = new AutoResetEvent(false);
RayCastWorld(start, direction, distance, hitAmounts, (r) =>
{
contactResults = r;
ev.Set();
});
ev.WaitOne(1000);
return(contactResults);
}
public override void CrossingFailure()
{
OpenMetaverse.Vector3 newPos = _position;
//place this object back into the region
if (_position.X > Constants.RegionSize - 1)
{
newPos.X = Constants.RegionSize - 1 /* - _actor.WorldBounds.Extents.X*/;
}
else if (_position.X <= 0f)
{
newPos.X = 0.0f /*_actor.WorldBounds.Extents.X*/;
}
if (_position.Y > Constants.RegionSize - 1)
{
newPos.Y = Constants.RegionSize - 1 /* - _actor.WorldBounds.Extents.Y*/;
}
else if (_position.Y <= 0f)
{
newPos.Y = 0.0f /*_actor.WorldBounds.Extents.Y*/;
}
//also make sure Z is above ground
float groundHeight = _scene.TerrainChannel.CalculateHeightAt(newPos.X, newPos.Y);
if (newPos.Z /* - _actor.WorldBounds.Extents.Z*/ < groundHeight)
{
newPos.Z = groundHeight /* + _actor.WorldBounds.Extents.Z*/ + 0.1f;
}
/*
* if (_dynActor != null)
* {
* bool wasKinematic = (_dynActor.Flags & PhysX.RigidDynamicFlags.Kinematic) != 0;
*
* _dynActor.GlobalPose = PhysUtil.PositionToMatrix(newPos, _rotation);
* _velocity = OpenMetaverse.Vector3.Zero;
* _angularVelocity = OpenMetaverse.Vector3.Zero;
*
* if (!wasKinematic)
* {
* _dynActor.AngularVelocity = PhysX.Math.Vector3.Zero;
* _dynActor.LinearVelocity = PhysX.Math.Vector3.Zero;
* _dynActor.PutToSleep();
* }
* }
*/
}
public static void Scale(OpenMetaverse.Vector3 scale, HacdConvexHull[] hulls)
{
foreach (HacdConvexHull hull in hulls)
{
PhysX.Math.Vector3[] vertices = hull.Vertices;
for (int i = 0; i < vertices.Length; ++i)
{
PhysX.Math.Vector3 vert = vertices[i];
vert.X *= scale.X;
vert.Y *= scale.Y;
vert.Z *= scale.Z;
vertices[i] = vert;
}
}
}
public PrepChildPrimAndLinkCmd(PhysxPrim parent, PhysxPrim child, OpenMetaverse.Vector3 localPos,
OpenMetaverse.Quaternion localRot)
{
Util.ThrowIfNull(parent, "parent");
Util.ThrowIfNull(child, "child");
_parent = parent;
_child = child;
_localPos = localPos;
_localRot = localRot;
_newShape = null;
_affectedPrims = new List <PhysxPrim> {
parent, child
};
_affectedPrims.AddRange(child.ChildShapes.Keys);
}
private void GetRayCastResults(OpenMetaverse.Vector3 start, OpenMetaverse.Vector3 direction,
float distance, int hitAmounts, Action <List <ContactResult> > result, PhysxScene scene)
{
int buffercount = 16;
int maxbuffercount = 1024;
PhysX.RaycastHit[] hits = null;
direction = OpenMetaverse.Vector3.Normalize(direction);
//Increase the buffer count if the call indicates overflow. Prevent infinite loops.
while (hits == null && buffercount <= maxbuffercount)
{
hits = SceneImpl.RaycastMultiple(PhysUtil.OmvVectorToPhysx(start),
PhysUtil.OmvVectorToPhysx(direction),
distance, PhysX.SceneQueryFlags.All,
buffercount,
null);
buffercount *= 2;
}
List <ContactResult> contactResults = new List <ContactResult>();
if (hits != null)
{
List <PhysX.RaycastHit> hitsSorted = new List <PhysX.RaycastHit>(hits);
hitsSorted.Sort((a, b) => a.Distance.CompareTo(b.Distance));
int count = 0;
foreach (PhysX.RaycastHit hit in hitsSorted)
{
contactResults.Add(new ContactResult()
{
Distance = hit.Distance,
FaceIndex = hit.FaceIndex,
CollisionActor = hit.Shape.Actor.UserData as PhysicsActor,
Position = PhysUtil.PhysxVectorToOmv(hit.Impact),
Normal = PhysUtil.PhysxVectorToOmv(hit.Normal),
});
if (++count >= hitAmounts)
{
break;
}
}
}
result(contactResults);
}
private void ChangeToChild(BasicPrim parent, OpenMetaverse.Vector3 localPos, OpenMetaverse.Quaternion localRot)
{
_parentPrim = parent;
//this prim no longer has its old shape or its actor
//_scene.PrimBecameChild(this);
//if we have children, we need to unref their shapes here, as our new parent (ours and our children)
//may require different shape types and will be rebuilt
//this.DeleteActor(_actor, _myShape, _isPhysical, DeleteActorFlags.UnrefChildShapes);
//_actor = null;
//_dynActor = null;
//_myShape = null;
_position = localPos;
_rotation = localRot;
}
private void DoPIDMoveTarget(float timeStep, uint frameNum)
{
float tau;
OpenMetaverse.Vector3 target;
float gtau;
lock (_properties)
{
tau = _properties.MoveTargetTau;
target = _properties.MoveTarget;
gtau = _properties.GrabTargetTau;
}
// Grab overrides move to target
if (gtau != 0)
{
return;
}
if (tau > MIN_TAU)
{
//i had a whole elaborate setup here to do this.. turns out the linden
//implementation was much simpler
OpenMetaverse.Vector3 distance = target - _position;
if (distance.LengthSquared() <= AT_TARGET_TOLERANCE_SQUARED)
{
if (_velocity != OpenMetaverse.Vector3.Zero)
{
StopLinearMovement();
return;
}
}
ChangeGravityIfNeeded();
_dynActor.AddForce(PhysUtil.OmvVectorToPhysx((distance * (1.0f / tau)) - _velocity), PhysX.ForceMode.VelocityChange, true);
}
else // Check for stop move to target - need to turn gravity back on if buoyancy isn't enabled
if ((target == OpenMetaverse.Vector3.Zero) && (tau == 0.0f))
{
ChangeGravityIfNeeded();
}
}
public BasicPrim(BasicPrim parent, BasicScene scene, PrimitiveBaseShape baseShape, OpenMetaverse.Vector3 pos,
OpenMetaverse.Quaternion rotation, /*PhysicsShape myShape, PhysX.RigidActor myActor,*/
bool isPhysical, IPhysicsProperties properties /*, CollisionGroupFlag collisionGroup*/)
{
_parentPrim = parent;
_scene = scene;
_pbs = baseShape;
_position = pos;
_rotation = rotation;
_isPhysical = isPhysical;
_properties = (BasicPhysicsProperties)properties;
//_collisionGroup = collisionGroup;
_acceleration = OpenMetaverse.Vector3.Zero;
_mass = OBJECT_DENSITY * _pbs.Scale.X * _pbs.Scale.Y * _pbs.Scale.Z;
//this.AssignActor(myActor, myShape, _isPhysical, DeleteActorFlags.None);
}
public CreateObjectCmd(PhysxPrim parent, string primName, OpenSim.Framework.PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position,
OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, OpenMetaverse.Vector3 velocity, OpenMetaverse.Vector3 angularVelocity,
float lod, PhysicsScene.AddPrimShapeFlags flags, Material material, byte[] serializedPhysicsProperties, byte[] serializedPhysicsShapes,
ulong interpolateTime)
{
_parent = parent;
_primName = primName;
_pbs = pbs;
_position = position;
_size = size;
_rotation = rotation;
_velocity = velocity;
_angularVelocity = angularVelocity;
_lod = lod;
_flags = flags;
_material = material;
_serializedPhysicsProperties = serializedPhysicsProperties;
_serializedPhysicsShapes = serializedPhysicsShapes;
_interpolateTime = interpolateTime;
}
public CreateObjectCmd(PhysxPrim parent, string primName, OpenSim.Framework.PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position,
OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, OpenMetaverse.Vector3 velocity, OpenMetaverse.Vector3 angularVelocity,
float lod, PhysicsScene.AddPrimShapeFlags flags, Material material, byte[] serializedPhysicsProperties, byte[] serializedPhysicsShapes,
ulong interpolateTime)
{
_parent = parent;
_primName = primName;
_pbs = pbs;
_position = position;
_size = size;
_rotation = rotation;
_velocity = velocity;
_angularVelocity = angularVelocity;
_lod = lod;
_flags = flags;
_material = material;
_serializedPhysicsProperties = serializedPhysicsProperties;
_serializedPhysicsShapes = serializedPhysicsShapes;
_interpolateTime = interpolateTime;
}
private bool DoGrabTarget(float timeStep, uint frameNum)
{
float tau;
OpenMetaverse.Vector3 target;
lock (_properties)
{
tau = _properties.GrabTargetTau;
target = _properties.GrabTarget;
}
if (tau > MIN_TAU)
{
_dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(-_angularVelocity * 0.9f), PhysX.ForceMode.VelocityChange, false);
OpenMetaverse.Vector3 distance = target - _position;
if (distance.LengthSquared() <= AT_TARGET_TOLERANCE_SQUARED)
{
if (_velocity != OpenMetaverse.Vector3.Zero)
{
StopLinearMovement();
return(true);
}
}
_grabDisableGravity = true;
ChangeGravityIfNeeded();
_dynActor.AddForce(PhysUtil.OmvVectorToPhysx((distance * (1.0f / tau)) - _velocity), PhysX.ForceMode.VelocityChange, true);
}
else
{
// Check for stop move to grab - need to turn gravity back on if buoyancy isn't enabled
if (tau == 0.0f)
{
_grabDisableGravity = false;
ChangeGravityIfNeeded();
return(false);
}
}
return(true);
}
private VelocityStatus CheckSyncVelocity(OpenMetaverse.Vector3 posDiff)
{
VelocityStatus status = VelocityStatus.NoChange;
//check my angular and linear velocity
OpenMetaverse.Vector3 newVel = PhysUtil.PhysxVectorToOmv(_dynActor.LinearVelocity);
if (newVel != _velocity)
{
if (newVel == OpenMetaverse.Vector3.Zero)
{
// we need to assume there is no acceleration acting on the prim anymore
// or our objects will float away instead of coming to rest in their final movement
_acceleration = OpenMetaverse.Vector3.Zero;
_velocity = OpenMetaverse.Vector3.Zero;
_lastAccelVelUpdate = 0;
status |= VelocityStatus.Zeroed;
}
else
{
//try to get a semi-accurate FPS to send the viewer the correct acceleration
ulong tickCountNow = Util.GetLongTickCount();
ulong timeDiff = tickCountNow - _lastAccelVelUpdate;
float fps;
if (_lastAccelVelUpdate == 0 || timeDiff <= 0)
{
fps = NOMINAL_FPS;
}
else
{
fps = 1.0f / (timeDiff * 0.001f);
}
var lastAccel = _acceleration;
var newAccel = (newVel - _velocity) * fps;
_velocity = newVel;
if (!lastAccel.ApproxEquals(newAccel, ACCEL_COMPARISON_TOLERANCE * _velocity.Length()))
{
//m_log.DebugFormat("Vel: {0} Accel: {1} Fps: {2}", _velocity, newAccel, fps);
_acceleration = newAccel;
status |= VelocityStatus.Changed;
}
_lastAccelVelUpdate = tickCountNow;
}
}
else
{
if (_velocity != OpenMetaverse.Vector3.Zero)
{
_acceleration = OpenMetaverse.Vector3.Zero;
_lastAccelVelUpdate = Util.GetLongTickCount();
status |= VelocityStatus.Changed;
}
}
if (status != VelocityStatus.NoChange)
{
_angularVelocity = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity);
}
else
{
OpenMetaverse.Vector3 newAngVel = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity);
//m_log.DebugFormat("AngVel: {0}, NewAngVel: {1}", _angularVelocity, newAngVel);
if (newAngVel == OpenMetaverse.Vector3.Zero)
{
if (newAngVel != _angularVelocity)
{
_angularVelocity = OpenMetaverse.Vector3.Zero;
status |= VelocityStatus.AngularChanged;
}
}
else if (!newAngVel.ApproxEquals(_angularVelocity, ANGULAR_VELOCITY_COMPARISON_TOLERANCE * _angularVelocity.Length()))
{
//Console.Out.WriteLine("Ang: {0}", _angularVelocity);
_angularVelocity = newAngVel;
status |= VelocityStatus.AngularChanged;
}
else
{
//Angular velocity hasnt changed or zeroed. BUT if angular velocity is set and
//the center of mass isnt at 0,0,0 and POS has changed we need to send an update
if (!_centerOfMassNearlyZero && !posDiff.ApproxEquals(OpenMetaverse.Vector3.Zero, POS_ANGULAR_VEL_TOLERANCE))
{
_angularVelocity = newAngVel;
status |= VelocityStatus.AngularPosChanged;
_useAngularVelocity = false;
}
else
{
if (_useAngularVelocity == false)
{
status |= VelocityStatus.AngularPosChanged;
_useAngularVelocity = true;
}
}
}
}
return status;
}
private void ChangeToChild(PhysxPrim parent, OpenMetaverse.Vector3 localPos, OpenMetaverse.Quaternion localRot)
{
_parentPrim = parent;
//this prim no longer has its old shape or its actor
_scene.PrimBecameChild(this);
//if we have children, we need to unref their shapes here, as our new parent (ours and our children)
//may require different shape types and will be rebuilt
this.DeleteActor(_actor, _myShape, _isPhysical, DeleteActorFlags.UnrefChildShapes);
_actor = null;
_dynActor = null;
_myShape = null;
_position = localPos;
_rotation = localRot;
}
private OpenMetaverse.Vector3 CheckSyncPosition()
{
//check my position
OpenMetaverse.Vector3 oldPos = _position;
_position = PhysUtil.DecomposeToPosition(_actor.GlobalPose);
return oldPos - _position;
}
private void CheckAvatarNotBelowGround()
{
float groundHeight = _scene.TerrainChannel.CalculateHeightAt(_position.X, _position.Y);
if (_position.Z < groundHeight)
{
_vForces = OpenMetaverse.Vector3.Zero;
_vGravity = OpenMetaverse.Vector3.Zero;
_vTarget = OpenMetaverse.Vector3.Zero;
//place the avatar a decimeter above the ground
_position.Z = groundHeight + (_height / 2.0f) + 0.1f;
_controller.Position = PhysUtil.OmvVectorToPhysx(_position);
}
}
public SetPositionCmd(PhysxPrim prim, OpenMetaverse.Vector3 newPos)
{
_prim = prim;
_newPosition = newPos;
}
private void CacheMassData()
{
_mass = _dynActor.Mass;
_centerOfMassLocalPose = PhysUtil.MatrixToPose(_dynActor.CenterOfMassLocalPose);
_massSpaceInertiaTensor = PhysUtil.PhysxVectorToOmv(_dynActor.MassSpaceInertiaTensor);
PhysX.Math.Vector3 bbExtents = _dynActor.WorldBounds.Extents;
float bbLen = bbExtents.Length() * 2;
if (_centerOfMassLocalPose.Position.Length() / bbLen > CENTER_OF_MASS_POS_TOLERANCE) //check if the center of mass is too far off in general
{
_centerOfMassNearlyZero = false;
return;
}
else if (Math.Abs(_centerOfMassLocalPose.Position.X / bbExtents.X) > CENTER_OF_MASS_POS_TOLERANCE) //check if X is too far off
{
_centerOfMassNearlyZero = false;
return;
}
else if (Math.Abs(_centerOfMassLocalPose.Position.Y / bbExtents.Y) > CENTER_OF_MASS_POS_TOLERANCE) //same with Y
{
_centerOfMassNearlyZero = false;
return;
}
else if (Math.Abs(_centerOfMassLocalPose.Position.Z / bbExtents.Z) > CENTER_OF_MASS_POS_TOLERANCE) //and Z
{
_centerOfMassNearlyZero = false;
return;
}
_centerOfMassNearlyZero = true;
}
public override void SyncWithPhysics(float timeStep, uint ticksSinceLastSimulate, uint frameNum)
{
if (_suspended)
{
// character is in the middle of a crossing. we do not simulate
_lastSync = (uint)Environment.TickCount;
return;
}
float secondsSinceLastSync = Math.Min(((uint)Environment.TickCount - _lastSync) * 0.001f, MAX_TIMESTEP);
//m_log.DebugFormat("[CHAR]: secondsSinceLastSync: {0}", secondsSinceLastSync);
//sometimes a single quantum doesnt show up here, and the calculation returns a zero
if (secondsSinceLastSync < MIN_TIMESTEP * 2)
{
secondsSinceLastSync = MIN_TIMESTEP * 2;
}
AccumulateGravity(secondsSinceLastSync);
DecayForces(secondsSinceLastSync);
OpenMetaverse.Vector3 cforces = _cForcesAreLocal ? _cForces * _rotation : _cForces;
cforces.Z = 0;
OpenMetaverse.Vector3 vCombined = (_vGravity + _vForces + cforces + this.VTargetWithRunAndRamp) * secondsSinceLastSync;
//m_log.DebugFormat("[CHAR]: vGrav: {0}, vForces: {1}, vTarget {2}", _vGravity, _vForces, this.VTargetWithRun);
if (vCombined == OpenMetaverse.Vector3.Zero)
{
SetVelocityAndRequestTerseUpdate(secondsSinceLastSync, OpenMetaverse.Vector3.Zero);
ReportCollisionsFromLastFrame(frameNum);
return;
}
OpenMetaverse.Vector3 lastPosition = _position;
PhysX.ControllerFlag flags = _controller.Move(PhysUtil.OmvVectorToPhysx(vCombined), TimeSpan.FromSeconds(secondsSinceLastSync), 0.001f, FILTERS);
_position = PhysUtil.PhysxVectorToOmv(_controller.Position);
_lastSync = (uint)Environment.TickCount;
//take into account any movement not accounted for by the other calculations
//this is due to collision
OpenMetaverse.Vector3 vColl = (_position - lastPosition) - vCombined;
//m_log.InfoFormat("vColl {0} {1} PosDiff: {2} Expected: {3}", vColl, flags, _position - lastPosition, vCombined);
//m_log.DebugFormat("[CHAR]: vColl: {0}", vColl);
bool collidingDown = (flags & PhysX.ControllerFlag.Down) != 0;
if (!collidingDown) _rideOnBehavior.AvatarNotStandingOnPrim();
//negative z in vcoll while colliding down is due to gravity/ground collision, dont report it
float gravityPushback = Math.Abs(_vGravity.Z) * secondsSinceLastSync;
if (collidingDown && vColl.Z > 0 && Math.Abs(vColl.Z - gravityPushback) < GRAVITY_PUSHBACK_DIFF_TOLERANCE) vColl.Z = 0;
//m_log.DebugFormat("[CHAR]: vColl: {0} gravityPushback {1} collidingDown:{2}", vColl, gravityPushback, collidingDown);
if (flags != 0)
{
_colliding = true;
if (collidingDown)
{
_collidingGround = true;
_flying = false;
_vGravity = OpenMetaverse.Vector3.Zero;
_vForces.Z = 0.0f;
_vTarget.Z = 0.0f;
}
else
{
_collidingGround = false;
//if we're colliding with anything but the ground, zero out other forces
_vForces = OpenMetaverse.Vector3.Zero;
}
}
else
{
_colliding = false;
_collidingGround = false;
}
if (frameNum % 3 == 0)
{
CheckAvatarNotBelowGround();
}
SetVelocityAndRequestTerseUpdate(secondsSinceLastSync, vColl);
ReportCollisionsFromLastFrame(frameNum);
if (!_position.ApproxEquals(lastPosition, POSITION_COMPARISON_TOLERANCE))
{
RequestPhysicsPositionUpdate();
}
}
private void ClearForces()
{
_velocity = OpenMetaverse.Vector3.Zero;
_angularVelocity = OpenMetaverse.Vector3.Zero;
_acceleration = OpenMetaverse.Vector3.Zero;
}
//public override void SetMaterial(IMaterial
public override void LinkToNewParent(PhysicsActor obj, OpenMetaverse.Vector3 localPos, OpenMetaverse.Quaternion localRot)
{
lock (_terseConsistencyLock)
{
_position = localPos;
_rotation = localRot;
}
_scene.QueueCommand(new Commands.PrepChildPrimAndLinkCmd((PhysxPrim)obj, this, localPos, localRot));
}
public PhysxPrim(PhysxPrim parent, PhysxScene scene, PrimitiveBaseShape baseShape, OpenMetaverse.Vector3 pos,
OpenMetaverse.Quaternion rotation, PhysicsShape myShape, PhysX.RigidActor myActor,
bool isPhysical, IPhysicsProperties properties, CollisionGroupFlag collisionGroup)
{
_parentPrim = parent;
_scene = scene;
_pbs = baseShape;
_position = pos;
_rotation = rotation;
_isPhysical = isPhysical;
_properties = (PhysicsProperties)properties;
_collisionGroup = collisionGroup;
this.AssignActor(myActor, myShape, _isPhysical, DeleteActorFlags.None);
if (_properties.VehicleProps != null && _properties.VehicleProps.Type != VehicleType.None)
{
//init dynamics
CheckCreateVehicleDynamics();
}
}
public Vector3(OMV_Vector3 vector)
{
x = vector.X;
y = vector.Y;
z = vector.Z;
}
private void DoResumeInterpolation()
{
//indicates we're resuming without suspending
if (_suspendedOn == 0) return;
ulong elapsed = Math.Min(OpenSim.Framework.Util.GetLongTickCount() - _suspendedOn, MAX_INTERPOLATION_TIME);
//nothing to do
if (elapsed == 0) return;
//interpolate
OpenMetaverse.Vector3 oldPos = _position;
_position = _position + (_velocity * (elapsed / 1000.0f));
_dynActor.GlobalPose = PhysUtil.PositionToMatrix(_position, _rotation);
EnsureObjectAboveGround();
if (TryRezDepenetrate() != PenetrationStatus.Ok)
{
//back out we're slammed into something
_position = oldPos;
_dynActor.GlobalPose = PhysUtil.PositionToMatrix(_position, _rotation);
}
}
internal void SetInitialVelocities(OpenMetaverse.Vector3 rootVelocity, OpenMetaverse.Vector3 rootAngularVelocity)
{
if (IsFreeDynamic || _suspended)
{
_velocity = rootVelocity;
_angularVelocity = rootAngularVelocity;
if (IsFreeDynamic)
{
_dynActor.LinearVelocity = PhysUtil.OmvVectorToPhysx(rootVelocity);
_dynActor.AngularVelocity = PhysUtil.OmvVectorToPhysx(rootAngularVelocity);
}
}
}
/// <summary>
/// Unlinks this prim from its parent
/// </summary>
/// <param name="newWorldPosition">The new position for the new physics actor</param>
/// <param name="newWorldRotation">The new rotation for the new physics actor</param>
internal void UnlinkFromParent(OpenMetaverse.Vector3 newWorldPosition,
OpenMetaverse.Quaternion newWorldRotation)
{
if (_parentPrim == null) return;
bool physical = _parentPrim.IsPhysical;
_position = newWorldPosition;
_rotation = newWorldRotation;
PhysicsShape childShape = _parentPrim.UnlinkChild(this);
_parentPrim = null;
if (childShape != null)
{
this.RebuildPhysxActorWithNewShape(childShape, null, physical, true);
}
}
private void UpdateDynamicForVelocityTargets(float timeStep)
{
if (IsFreeDynamic)
{
OpenMetaverse.Vector3 angularVelocityTarget;
lock (_properties)
{
angularVelocityTarget = _properties.AngularVelocityTarget;
}
if (angularVelocityTarget != OpenMetaverse.Vector3.Zero)
{
//physical omegas should be relative to the object
angularVelocityTarget *= _rotation;
if (_centerOfMassLocalPose == Pose.Identity)
{
//subtract the target velocity from our current to get the difference that needs to be applied
OpenMetaverse.Vector3 currAngVel = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity);
_dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(angularVelocityTarget - currAngVel), PhysX.ForceMode.VelocityChange, true);
}
else
{
OpenMetaverse.Quaternion comRot = _centerOfMassLocalPose.Rotation;
OpenMetaverse.Quaternion q = GetRotationFromActor() * comRot;
OpenMetaverse.Vector3 tensor = _massSpaceInertiaTensor;
OpenMetaverse.Vector3 t = (tensor * (angularVelocityTarget * OpenMetaverse.Quaternion.Inverse(q))) * q;
//subtract the target velocity from our current to get the difference that needs to be applied
OpenMetaverse.Vector3 currAngVel = PhysUtil.PhysxVectorToOmv(_dynActor.AngularVelocity);
_dynActor.AddTorque(PhysUtil.OmvVectorToPhysx(t - currAngVel), PhysX.ForceMode.VelocityChange, true);
}
}
if (_properties.Force != OpenMetaverse.Vector3.Zero)
{
if (_properties.ForceIsLocal)
{
_dynActor.AddLocalForceAtLocalPosition(PhysUtil.OmvVectorToPhysx(_properties.Force * timeStep),
PhysUtil.OmvVectorToPhysx(_centerOfMassLocalPose.Position), PhysX.ForceMode.Impulse, true);
}
else
{
_dynActor.AddForce(PhysUtil.OmvVectorToPhysx(_properties.Force * timeStep), PhysX.ForceMode.Impulse, true);
}
}
if (this.WantsGravity)
{
// Buoyancy is bascially a negative gravity multiplier where B=1.0 exerts -1G. The Delta G is then
// the sum of negative bouyancy plus the gravity multiplier.
float DG = -_properties.Buoyancy + _properties.Material.GravityMultiplier;
// The compensating force to exert (since gravity is enabled) is the delta G minus 1G
float force = (DG - 1.0f) * Settings.Instance.Gravity * _mass;
// If a compensating force is to be exerted, do so only while the object has very little Z-velocity or
// some positional movement. Using velocity alone fails because with PhysX a high GM object jitters intensely
// when stopped by terrain or another static prim, and so its velocity is very high.
if (force != 0)
{
// Compute short term smoothed position delta.
_positiondeltaz = _positiondeltaz * 0.8f + (_position.Z - _lastposition.Z) * 0.2f;
// A higher velocity may be indicative of PhysX jitter.
if (Math.Abs(_dynActor.LinearVelocity.Z) > THRESHOLD_JITTER_DETECT)
{
// If the positional jitter induced by PhysX is below a threshold, then the
// object is beig stopped by stationary object or terrain. Lower the force
// to eliminate the jitter altogether, but permits movement should the obstruction
// move aside.
if (Math.Abs(_positiondeltaz) < THRESHOLD_JITTER_DETECT * timeStep)
force = Util.Clamp(Math.Sign(DG - 1.0f) * Settings.Instance.Gravity * _mass * 2.0f, -force, force);
}
// m_log.DebugFormat("[Buoyancy] f={0} zvel={1} dp={2} at {3} b={4} gm={5}", force, _dynActor.LinearVelocity.Z, _positiondeltaz, _position, _properties.Buoyancy, _properties.Material.GravityMultiplier);
_dynActor.AddForce(new PhysX.Math.Vector3(0.0f, 0.0f, force * timeStep), PhysX.ForceMode.Impulse, true);
_lastposition = _position;
}
}
}
}
private float CalculateDepenetrationZOffset(OpenMetaverse.Vector3 pos, PhysX.Geometry avaGeom, PhysX.Shape avaShape)
{
const int MAX_ITERATIONS = 8;
const float PUSH_MULTIPLIER = 1.5F;
float pushFactor = 0.1f;
OpenMetaverse.Vector3 offset = OpenMetaverse.Vector3.Zero;
bool foundOverlap = false;
//constant from looking at the rot returned from the live avatar,
//remember that capsules are always upright, and z rotations don't have an effect
//on their geometry
OpenMetaverse.Quaternion capsuleRot = new OpenMetaverse.Quaternion(0f, -0.7071069f, 0f, 0.7071067f);
for (int i = 0; i < MAX_ITERATIONS; i++)
{
foundOverlap = false;
OpenMetaverse.Vector3 translatedPose = pos + offset;
PhysX.Shape[] overlap = _scene.SceneImpl.OverlapMultiple(avaGeom, PhysUtil.PositionToMatrix(translatedPose, capsuleRot));
if (overlap == null)
{
foundOverlap = true;
}
else
{
foreach (var shape in overlap)
{
if (shape != avaShape && !ShapeIsVolumeDetect(shape))
{
foundOverlap = true;
break;
}
}
}
if (foundOverlap && i + 1 < MAX_ITERATIONS)
{
offset += new OpenMetaverse.Vector3(0f, 0f, pushFactor);
pushFactor *= PUSH_MULTIPLIER;
}
else
{
break;
}
}
if (foundOverlap == false && offset != OpenMetaverse.Vector3.Zero)
{
return offset.Z;
}
return 0.0f;
}
private void SetVelocityAndRequestTerseUpdate(float secondsSinceLastSync, OpenMetaverse.Vector3 vColl)
{
OpenMetaverse.Vector3 cforces = _cForcesAreLocal ? _cForces * _rotation : _cForces;
cforces.Z = 0;
OpenMetaverse.Vector3 oldVelocity = _velocity;
_velocity = (_vGravity + _vForces + cforces + this.VTargetWithRunAndRamp + vColl);
if (_velocity == OpenMetaverse.Vector3.Zero && oldVelocity != OpenMetaverse.Vector3.Zero)
{
_acceleration = OpenMetaverse.Vector3.Zero;
RequestPhysicsterseUpdate();
}
else
{
OpenMetaverse.Vector3 velDiff = _velocity - oldVelocity;
OpenMetaverse.Vector3 accel = velDiff / secondsSinceLastSync;
if (!accel.ApproxEquals(_acceleration, ACCELERATION_COMPARISON_TOLERANCE))
{
_acceleration = accel;
RequestPhysicsterseUpdate();
//m_log.DebugFormat("Avatar Terse Vel: {0} Accel: {1} Sync: {2}", _velocity, _acceleration, secondsSinceLastSync);
//m_log.DebugFormat("Vel Breakdown: vGravity {0} vForces {1} vTarget {2} vColl {3}", _vGravity, _vForces, this.VTargetWithRun, vColl);
}
}
}
public override void UpdateOffsetPosition(OpenMetaverse.Vector3 newOffset, OpenMetaverse.Quaternion rotOffset)
{
//Offset updates are only handled for child prims. Upstream a non child move is
//handled by setting our position directly
if (IsChild)
{
_position = newOffset;
_rotation = rotOffset;
_parentPrim.ChildPrimOffsetChanged(this, newOffset, rotOffset);
}
}
public ExpVector3 (Parser yyp, float x , float y , float z ):base(((PCParser
)yyp)){ this . val = new OpenMetaverse . Vector3 ( x , y , z );
}
private void DecayForces(float secondsSinceLastSync)
{
if (_vForces != OpenMetaverse.Vector3.Zero)
{
if (_vTarget != OpenMetaverse.Vector3.Zero)
{
if (!_flying)
{
//user movement instantly cancels any x or y axis movement, but
//it does not cancel z axis movement while jumping. This allows the user to have
//a nice jump while walking
_vForces.X = 0f;
_vForces.Y = 0f;
}
else
{
_vForces = OpenMetaverse.Vector3.Zero;
}
}
else
{
//decay velocity in relation to velocity to badly mimic drag
OpenMetaverse.Vector3 decayForce;
if (_collidingGround)
{
decayForce = OpenMetaverse.Vector3.Multiply(_vForces, 2.0f * secondsSinceLastSync);
}
else
{
decayForce = OpenMetaverse.Vector3.Multiply(_vForces, 1.0f * secondsSinceLastSync);
}
_vForces -= decayForce;
if (_vForces.LengthSquared() < MIN_FORCE_MAG_BEFORE_ZEROING_SQUARED)
{
_vForces = OpenMetaverse.Vector3.Zero;
}
}
}
}
public override void AddForceSync(OpenMetaverse.Vector3 Force, OpenMetaverse.Vector3 forceOffset, ForceType type)
{
Force /= _mass;
switch (type)
{
case ForceType.ConstantLocalLinearForce:
_cForcesAreLocal = true;
_cForces = Force;
break;
case ForceType.ConstantGlobalLinearForce:
_cForcesAreLocal = false;
_cForces = Force;
break;
case ForceType.GlobalLinearImpulse:
_vForces += Force;
break;
case ForceType.LocalLinearImpulse:
_vForces += Force * _rotation;
break;
}
}
public PhysxCharacter(PhysxScene scene, float height, float radius,
OpenMetaverse.Vector3 position, OpenMetaverse.Quaternion rotation,
bool flying, OpenMetaverse.Vector3 initialVelocity)
{
_scene = scene;
_radius = Math.Max(radius, 0.2f);
/*
* The capsule is defined as a position, a vertical height, and a radius. The height is the distance between the
* two sphere centers at the end of the capsule. In other words:
*
* p = pos (returned by controller)
* h = height
* r = radius
*
* p = center of capsule
* top sphere center = p.y + h*0.5
* bottom sphere center = p.y - h*0.5
* top capsule point = p.y + h*0.5 + r
* bottom capsule point = p.y - h*0.5 - r
*/
_height = height;
_flying = flying;
float volume = (float)(Math.PI * Math.Pow(_radius, 2) * this.CapsuleHeight);
_mass = CHARACTER_DENSITY * volume;
_position = position;
_rotation = rotation;
_hitReportDelegator = new UserControllerHitReportDelegator();
_hitReportDelegator.OnShapeHitCallback += this.OnShapeHit;
_hitReportDelegator.OnControllerHitCallback += this.OnControllerHit;
PhysX.CapsuleControllerDesc controllerDesc = new PhysX.CapsuleControllerDesc
{
Height = this.CapsuleHeight,
Radius = _radius,
StepOffset = STEP_OFFSET,
UpDirection = new PhysX.Math.Vector3(0.0f, 0.0f, 1.0f),
Position = PhysUtil.OmvVectorToPhysx(position),
Material = scene.DEFAULT_MATERIAL,
InteractionMode = PhysX.CCTInteractionMode.Include,
SlopeLimit = (float)Math.Cos(OpenMetaverse.Utils.DEG_TO_RAD * MAX_WALKABLE_SLOPE),
ContactOffset = CONTACT_OFFSET,
Callback = _hitReportDelegator,
BehaviorCallback = _rideOnBehavior
};
_controller = _scene.ControllerManager.CreateController<PhysX.CapsuleController>(controllerDesc);
_controller.Actor.UserData = this;
DoZDepenetration();
_controller.ShapeFilterData = CollisionGroup.GetFilterData((uint)(PhysX.PairFlag.NotifyTouchFound | PhysX.PairFlag.NotifyTouchLost),
0, CollisionGroupFlag.Character);
_lastSync = (uint)Environment.TickCount;
_vTarget = initialVelocity;
_velocity = initialVelocity;
if (_vTarget != OpenMetaverse.Vector3.Zero)
{
//hack to continue at velocity until the controller picks up
_lastVelocityNonZero = OpenSim.Framework.Util.GetLongTickCount() - VELOCITY_RAMPUP_TIME;
}
}
public override void DelinkFromParent(OpenMetaverse.Vector3 newWorldPosition, OpenMetaverse.Quaternion newWorldRotation)
{
lock (_terseConsistencyLock)
{
_position = newWorldPosition;
_rotation = newWorldRotation;
}
_scene.QueueCommand(new Commands.UnlinkFromParentCmd(this, _parentPrim, newWorldPosition, newWorldRotation));
}
public override void CrossingFailure()
{
_scene.QueueCommand(
new Commands.GenericSyncCmd(this,
(PhysxScene scene) =>
{
OpenMetaverse.Vector3 newPos = _position;
//place this object back into the region
if (_position.X > Constants.RegionSize - 1)
{
newPos.X = Constants.RegionSize - 1 - _actor.WorldBounds.Extents.X;
}
else if (_position.X <= 0f)
{
newPos.X = _actor.WorldBounds.Extents.X;
}
if (_position.Y > Constants.RegionSize - 1)
{
newPos.Y = Constants.RegionSize - 1 - _actor.WorldBounds.Extents.Y;
}
else if (_position.Y <= 0f)
{
newPos.Y = _actor.WorldBounds.Extents.Y;
}
//also make sure Z is above ground
float groundHeight = _scene.TerrainChannel.CalculateHeightAt(newPos.X, newPos.Y);
if (newPos.Z - _actor.WorldBounds.Extents.Z < groundHeight)
{
newPos.Z = groundHeight + _actor.WorldBounds.Extents.Z + 0.1f;
}
if (_dynActor != null)
{
bool wasKinematic = (_dynActor.Flags & PhysX.RigidDynamicFlags.Kinematic) != 0;
_dynActor.GlobalPose = PhysUtil.PositionToMatrix(newPos, _rotation);
_velocity = OpenMetaverse.Vector3.Zero;
_angularVelocity = OpenMetaverse.Vector3.Zero;
if (!wasKinematic)
{
_dynActor.AngularVelocity = PhysX.Math.Vector3.Zero;
_dynActor.LinearVelocity = PhysX.Math.Vector3.Zero;
_dynActor.PutToSleep();
}
}
}
));
}
