a few more changes on avatar collider
parent
c6430b14db
commit
80639ace95
|
@ -74,7 +74,6 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
private Vector3 _position;
|
||||
private Vector3 _zeroPosition;
|
||||
private bool _zeroFlag = false;
|
||||
private Vector3 _velocity;
|
||||
private Vector3 _target_velocity;
|
||||
private Vector3 _acceleration;
|
||||
|
@ -90,11 +89,15 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
public float PID_D;
|
||||
public float PID_P;
|
||||
|
||||
private float timeStep;
|
||||
private float invtimeStep;
|
||||
|
||||
private float m_feetOffset = 0;
|
||||
private float feetOff = 0;
|
||||
private float feetSZ = 0.5f;
|
||||
const float feetScale = 0.8f;
|
||||
private float boneOff = 0;
|
||||
private float m_lastVelocitySqr = 0;
|
||||
|
||||
public float walkDivisor = 1.3f;
|
||||
public float runDivisor = 0.8f;
|
||||
|
@ -103,6 +106,9 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
private bool m_iscollidingGround = false;
|
||||
private bool m_iscollidingObj = false;
|
||||
private bool m_alwaysRun = false;
|
||||
|
||||
private bool _zeroFlag = false;
|
||||
|
||||
private int m_requestedUpdateFrequency = 0;
|
||||
private uint m_localID = 0;
|
||||
public bool m_returnCollisions = false;
|
||||
|
@ -120,6 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
int m_colliderfilter = 0;
|
||||
int m_colliderGroundfilter = 0;
|
||||
int m_colliderObjectfilter = 0;
|
||||
bool m_collisionException = false;
|
||||
|
||||
// Default we're a Character
|
||||
private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
|
||||
|
@ -132,10 +139,11 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
// we do land collisions not ode | CollisionCategories.Land);
|
||||
public IntPtr Body = IntPtr.Zero;
|
||||
private OdeScene _parent_scene;
|
||||
public IntPtr topbox = IntPtr.Zero;
|
||||
public IntPtr midbox = IntPtr.Zero;
|
||||
public IntPtr feetbox = IntPtr.Zero;
|
||||
public IntPtr bonebox = IntPtr.Zero;
|
||||
private IntPtr topbox = IntPtr.Zero;
|
||||
private IntPtr midbox = IntPtr.Zero;
|
||||
private IntPtr feetbox = IntPtr.Zero;
|
||||
private IntPtr bbox = IntPtr.Zero;
|
||||
public IntPtr collider = IntPtr.Zero;
|
||||
|
||||
public IntPtr Amotor = IntPtr.Zero;
|
||||
|
||||
|
@ -143,6 +151,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
|
||||
|
||||
|
||||
public int m_eventsubscription = 0;
|
||||
private int m_cureventsubscription = 0;
|
||||
private CollisionEventUpdate CollisionEventsThisFrame = null;
|
||||
|
@ -160,6 +169,9 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
{
|
||||
m_uuid = UUID.Random();
|
||||
|
||||
timeStep = parent_scene.ODE_STEPSIZE;
|
||||
invtimeStep = 1 / timeStep;
|
||||
|
||||
if (pos.IsFinite())
|
||||
{
|
||||
if (pos.Z > 99999f)
|
||||
|
@ -208,8 +220,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
|
||||
|
||||
PID_D = basePID_D * m_mass / parent_scene.ODE_STEPSIZE;
|
||||
PID_P = basePID_P * m_mass / parent_scene.ODE_STEPSIZE;
|
||||
PID_D = basePID_D * m_mass * invtimeStep;
|
||||
PID_P = basePID_P * m_mass * invtimeStep;
|
||||
|
||||
m_isPhysical = false; // current status: no ODE information exists
|
||||
|
||||
|
@ -292,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
set
|
||||
{
|
||||
flying = value;
|
||||
// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
|
||||
// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -336,25 +348,25 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
get { return m_iscollidingGround; }
|
||||
set
|
||||
{
|
||||
/* we now control this
|
||||
if (value)
|
||||
{
|
||||
m_colliderGroundfilter += 2;
|
||||
if (m_colliderGroundfilter > 2)
|
||||
m_colliderGroundfilter = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_colliderGroundfilter--;
|
||||
if (m_colliderGroundfilter < 0)
|
||||
m_colliderGroundfilter = 0;
|
||||
}
|
||||
/* we now control this
|
||||
if (value)
|
||||
{
|
||||
m_colliderGroundfilter += 2;
|
||||
if (m_colliderGroundfilter > 2)
|
||||
m_colliderGroundfilter = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_colliderGroundfilter--;
|
||||
if (m_colliderGroundfilter < 0)
|
||||
m_colliderGroundfilter = 0;
|
||||
}
|
||||
|
||||
if (m_colliderGroundfilter == 0)
|
||||
m_iscollidingGround = false;
|
||||
else
|
||||
m_iscollidingGround = true;
|
||||
*/
|
||||
if (m_colliderGroundfilter == 0)
|
||||
m_iscollidingGround = false;
|
||||
else
|
||||
m_iscollidingGround = true;
|
||||
*/
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -386,7 +398,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
else
|
||||
m_iscollidingObj = true;
|
||||
|
||||
// m_iscollidingObj = value;
|
||||
// m_iscollidingObj = value;
|
||||
|
||||
if (m_iscollidingObj)
|
||||
m_pidControllerActive = false;
|
||||
|
@ -634,8 +646,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
get { return m_orientation; }
|
||||
set
|
||||
{
|
||||
// fakeori = value;
|
||||
// givefakeori++;
|
||||
// fakeori = value;
|
||||
// givefakeori++;
|
||||
|
||||
value.Normalize();
|
||||
AddChange(changes.Orientation, value);
|
||||
|
@ -690,6 +702,46 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
AddChange(changes.Momentum, momentum);
|
||||
}
|
||||
|
||||
private void ajustCollider()
|
||||
{
|
||||
float vq = _velocity.LengthSquared();
|
||||
if (m_lastVelocitySqr != vq)
|
||||
{
|
||||
m_lastVelocitySqr = vq;
|
||||
if (vq > 100.0f)
|
||||
{
|
||||
Vector3 off = _velocity;
|
||||
float t = 0.5f * timeStep;
|
||||
off = off * t;
|
||||
d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
|
||||
off.X = 2.0f * (m_size.X + Math.Abs(off.X));
|
||||
off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
|
||||
off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
|
||||
d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
|
||||
|
||||
d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
|
||||
d.GeomSetCategoryBits(topbox, 0);
|
||||
d.GeomSetCollideBits(topbox, 0);
|
||||
d.GeomSetCategoryBits(midbox, 0);
|
||||
d.GeomSetCollideBits(midbox, 0);
|
||||
d.GeomSetCategoryBits(feetbox, 0);
|
||||
d.GeomSetCollideBits(feetbox, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
d.GeomSetCategoryBits(bbox, 0);
|
||||
d.GeomSetCollideBits(bbox, 0);
|
||||
d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
|
||||
d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
|
||||
d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
|
||||
{
|
||||
// sizes one day should came from visual parameters
|
||||
|
@ -697,7 +749,6 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
float sy = m_size.Y;
|
||||
float sz = m_size.Z;
|
||||
|
||||
|
||||
float topsx = sx * 0.9f;
|
||||
float midsx = sx;
|
||||
float feetsx = sx * feetScale;
|
||||
|
@ -732,21 +783,17 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
_parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
|
||||
|
||||
feetbox = d.CreateBox(_parent_scene.CharsSpace, feetsx, feetsy, feetsz);
|
||||
d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
|
||||
collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
|
||||
d.HashSpaceSetLevels(collider, -4, 3);
|
||||
d.SpaceSetSublevel(collider, 3);
|
||||
d.SpaceSetCleanup(collider, false);
|
||||
d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
|
||||
|
||||
midbox = d.CreateBox(_parent_scene.CharsSpace, midsx, midsy, midsz);
|
||||
d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
|
||||
|
||||
topbox = d.CreateBox(_parent_scene.CharsSpace, topsx, topsy, topsz);
|
||||
d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
|
||||
|
||||
bonebox = d.CreateBox(_parent_scene.CharsSpace, bonesx, bonesy, bonesz);
|
||||
d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories);
|
||||
d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags);
|
||||
feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
|
||||
midbox = d.CreateBox(collider, midsx, midsy, midsz);
|
||||
topbox = d.CreateBox(collider, topsx, topsy, topsz);
|
||||
bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
|
||||
|
||||
m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass
|
||||
|
||||
|
@ -758,9 +805,13 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
Body = d.BodyCreate(_parent_scene.world);
|
||||
|
||||
_zeroFlag = false;
|
||||
m_collisionException = false;
|
||||
m_pidControllerActive = true;
|
||||
m_freemove = false;
|
||||
|
||||
_velocity = Vector3.Zero;
|
||||
m_lastVelocitySqr = 0;
|
||||
|
||||
d.BodySetAutoDisableFlag(Body, false);
|
||||
d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
|
||||
|
||||
|
@ -772,12 +823,14 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
d.GeomSetBody(feetbox, Body);
|
||||
d.GeomSetBody(midbox, Body);
|
||||
d.GeomSetBody(topbox, Body);
|
||||
d.GeomSetBody(bonebox, Body);
|
||||
d.GeomSetBody(bbox, Body);
|
||||
|
||||
d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
|
||||
d.GeomSetOffsetPosition(midbox, 0, 0, midz);
|
||||
d.GeomSetOffsetPosition(topbox, 0, 0, topz);
|
||||
d.GeomSetOffsetPosition(bonebox, 0, 0, m_feetOffset);
|
||||
|
||||
ajustCollider();
|
||||
|
||||
|
||||
// The purpose of the AMotor here is to keep the avatar's physical
|
||||
// surrogate from rotating while moving
|
||||
|
@ -841,44 +894,109 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
if (topbox != IntPtr.Zero)
|
||||
{
|
||||
_parent_scene.actor_name_map.Remove(topbox);
|
||||
_parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
|
||||
_parent_scene.waitForSpaceUnlock(collider);
|
||||
d.GeomDestroy(topbox);
|
||||
topbox = IntPtr.Zero;
|
||||
}
|
||||
if (midbox != IntPtr.Zero)
|
||||
{
|
||||
_parent_scene.actor_name_map.Remove(midbox);
|
||||
_parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
|
||||
_parent_scene.waitForSpaceUnlock(collider);
|
||||
d.GeomDestroy(midbox);
|
||||
midbox = IntPtr.Zero;
|
||||
}
|
||||
if (feetbox != IntPtr.Zero)
|
||||
{
|
||||
_parent_scene.actor_name_map.Remove(feetbox);
|
||||
_parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
|
||||
_parent_scene.waitForSpaceUnlock(collider);
|
||||
d.GeomDestroy(feetbox);
|
||||
feetbox = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (bonebox != IntPtr.Zero)
|
||||
if (bbox != IntPtr.Zero)
|
||||
{
|
||||
_parent_scene.actor_name_map.Remove(bonebox);
|
||||
_parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
|
||||
d.GeomDestroy(bonebox);
|
||||
bonebox = IntPtr.Zero;
|
||||
_parent_scene.actor_name_map.Remove(bbox);
|
||||
_parent_scene.waitForSpaceUnlock(collider);
|
||||
d.GeomDestroy(bbox);
|
||||
bbox = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (collider != IntPtr.Zero)
|
||||
{
|
||||
d.SpaceDestroy(collider);
|
||||
collider = IntPtr.Zero;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
|
||||
public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
|
||||
{
|
||||
float sin = 2.0f * rot.Z * rot.W;
|
||||
float cos = rot.W * rot.W - rot.Z * rot.Z;
|
||||
float tx = x;
|
||||
|
||||
x = tx * cos - y * sin;
|
||||
y = tx * sin + y * cos;
|
||||
}
|
||||
public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
|
||||
{
|
||||
float tx = x;
|
||||
x = tx * cos - y * sin;
|
||||
y = tx * sin + y * cos;
|
||||
}
|
||||
public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
|
||||
{
|
||||
float tx = x;
|
||||
x = tx * cos + y * sin;
|
||||
y = -tx * sin + y * cos;
|
||||
}
|
||||
|
||||
public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
|
||||
{
|
||||
float sin = - 2.0f * rot.Z * rot.W;
|
||||
float cos = rot.W * rot.W - rot.Z * rot.Z;
|
||||
float tx = x;
|
||||
|
||||
x = tx * cos - y * sin;
|
||||
y = tx * sin + y * cos;
|
||||
}
|
||||
|
||||
|
||||
public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
|
||||
{
|
||||
feetcollision = false;
|
||||
if (m_collisionException)
|
||||
return false;
|
||||
|
||||
if (me == bonebox) // inner bone
|
||||
if (me == bbox) // if moving fast
|
||||
{
|
||||
if (contact.pos.Z - _position.Z < boneOff)
|
||||
IsColliding = true;
|
||||
return true;
|
||||
// force a full inelastic collision
|
||||
m_collisionException = true;
|
||||
|
||||
Vector3 off = m_size * 0.5f;
|
||||
off.X += contact.depth;
|
||||
off.Y += contact.depth;
|
||||
off.Z += contact.depth;
|
||||
if (reverse)
|
||||
{
|
||||
off.X *= -contact.normal.X;
|
||||
off.Y *= -contact.normal.Y;
|
||||
off.Z *= -contact.normal.Z;
|
||||
}
|
||||
else
|
||||
{
|
||||
off.X *= contact.normal.X;
|
||||
off.Y *= contact.normal.Y;
|
||||
off.Z *= contact.normal.Z;
|
||||
}
|
||||
|
||||
off.X += contact.pos.X;
|
||||
off.Y += contact.pos.Y;
|
||||
off.Z += contact.pos.Z;
|
||||
|
||||
_position = off;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (me == topbox) // keep a box head
|
||||
|
@ -890,6 +1008,21 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
if (me == midbox)
|
||||
{
|
||||
if (Math.Abs(contact.normal.Z) > 0.95f)
|
||||
{
|
||||
float nz = contact.normal.Z;
|
||||
if (!reverse)
|
||||
nz = -nz;
|
||||
|
||||
if (nz > 0)
|
||||
return true; // missed head TODO
|
||||
|
||||
// missed feet collision?
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
t = offx * offx + offy * offy;
|
||||
t = (float)Math.Sqrt(t);
|
||||
t = 1 / t;
|
||||
|
@ -917,12 +1050,19 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
if (Math.Abs(contact.normal.Z) > 0.95f)
|
||||
{
|
||||
if (contact.normal.Z > 0)
|
||||
contact.normal.Z = 1.0f;
|
||||
else
|
||||
contact.normal.Z = -1.0f;
|
||||
contact.normal.X = 0.0f;
|
||||
contact.normal.Y = 0.0f;
|
||||
feetcollision = true;
|
||||
if (h < boneOff)
|
||||
IsColliding = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
float offz = h - feetOff; // distance from top of feetbox
|
||||
|
||||
if (offz > 0)
|
||||
|
@ -971,11 +1111,28 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
/// This is the avatar's movement control + PID Controller
|
||||
/// </summary>
|
||||
/// <param name="timeStep"></param>
|
||||
public void Move(float timeStep, List<OdeCharacter> defects)
|
||||
public void Move(List<OdeCharacter> defects)
|
||||
{
|
||||
if (Body == IntPtr.Zero)
|
||||
return;
|
||||
|
||||
if (m_collisionException)
|
||||
{
|
||||
d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
|
||||
d.BodySetLinearVel(Body, 0, 0, 0);
|
||||
|
||||
float v = _velocity.Length();
|
||||
if (v != 0)
|
||||
{
|
||||
v = 6.0f / v;
|
||||
_velocity = _velocity * v;
|
||||
d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
|
||||
}
|
||||
ajustCollider();
|
||||
m_collisionException = false;
|
||||
return;
|
||||
}
|
||||
|
||||
d.Vector3 dtmp = d.BodyGetPosition(Body);
|
||||
Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
|
||||
|
||||
|
@ -1049,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
//******************************************
|
||||
// colide with land
|
||||
|
||||
d.AABB aabb;
|
||||
d.GeomGetAABB(feetbox, out aabb);
|
||||
float chrminZ = aabb.MinZ - 0.02f; // move up a bit
|
||||
|
@ -1095,8 +1253,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
contact.Position.X = localpos.X;
|
||||
contact.Position.Y = localpos.Y;
|
||||
contact.Position.Z = chrminZ;
|
||||
contact.SurfaceNormal.X = 0f;
|
||||
contact.SurfaceNormal.Y = 0f;
|
||||
contact.SurfaceNormal.X = 0.0f;
|
||||
contact.SurfaceNormal.Y = 0.0f;
|
||||
contact.SurfaceNormal.Z = -1f;
|
||||
contact.RelativeSpeed = -vel.Z;
|
||||
contact.CharacterFeet = true;
|
||||
|
@ -1118,6 +1276,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
m_iscollidingGround = false;
|
||||
}
|
||||
|
||||
|
||||
//******************************************
|
||||
|
||||
bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
|
||||
|
@ -1253,21 +1412,58 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
}
|
||||
|
||||
// update our local ideia of position velocity and aceleration
|
||||
// _position = localpos;
|
||||
_position = localpos;
|
||||
|
||||
if (_zeroFlag)
|
||||
{
|
||||
_velocity = Vector3.Zero;
|
||||
_acceleration = Vector3.Zero;
|
||||
m_rotationalVelocity = Vector3.Zero;
|
||||
}
|
||||
else
|
||||
{
|
||||
_acceleration = _velocity; // previus velocity
|
||||
_velocity = vel;
|
||||
_acceleration = (vel - _acceleration) / timeStep;
|
||||
Vector3 a =_velocity; // previus velocity
|
||||
SetSmooth(ref _velocity, ref vel, 2);
|
||||
a = (_velocity - a) * invtimeStep;
|
||||
SetSmooth(ref _acceleration, ref a, 2);
|
||||
|
||||
dtmp = d.BodyGetAngularVel(Body);
|
||||
m_rotationalVelocity.X = 0f;
|
||||
m_rotationalVelocity.Y = 0f;
|
||||
m_rotationalVelocity.Z = dtmp.Z;
|
||||
Math.Round(m_rotationalVelocity.Z,3);
|
||||
}
|
||||
|
||||
ajustCollider();
|
||||
}
|
||||
|
||||
public void round(ref Vector3 v, int digits)
|
||||
{
|
||||
v.X = (float)Math.Round(v.X, digits);
|
||||
v.Y = (float)Math.Round(v.Y, digits);
|
||||
v.Z = (float)Math.Round(v.Z, digits);
|
||||
}
|
||||
|
||||
public void SetSmooth(ref Vector3 dst, ref Vector3 value)
|
||||
{
|
||||
dst.X = 0.1f * dst.X + 0.9f * value.X;
|
||||
dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
|
||||
dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
|
||||
}
|
||||
|
||||
public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
|
||||
{
|
||||
dst.X = 0.4f * dst.X + 0.6f * value.X;
|
||||
dst.X = (float)Math.Round(dst.X, rounddigits);
|
||||
|
||||
dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
|
||||
dst.Y = (float)Math.Round(dst.Y, rounddigits);
|
||||
|
||||
dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
|
||||
dst.Z = (float)Math.Round(dst.Z, rounddigits);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Updates the reported position and velocity.
|
||||
/// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
|
||||
|
@ -1394,10 +1590,11 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
|
||||
|
||||
_parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[bonebox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
|
||||
_parent_scene.AddCharacter(this);
|
||||
}
|
||||
else
|
||||
|
@ -1450,13 +1647,16 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
_position.Z + (m_size.Z - oldsz) * 0.5f);
|
||||
|
||||
Velocity = Vector3.Zero;
|
||||
|
||||
|
||||
_parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[bonebox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
|
||||
_parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
|
||||
}
|
||||
m_freemove = false;
|
||||
m_collisionException = false;
|
||||
m_pidControllerActive = true;
|
||||
}
|
||||
else
|
||||
|
@ -1565,6 +1765,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
if (Body != IntPtr.Zero)
|
||||
d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
|
||||
ajustCollider();
|
||||
}
|
||||
|
||||
private void donullchange()
|
||||
|
@ -1573,7 +1774,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
public bool DoAChange(changes what, object arg)
|
||||
{
|
||||
if (topbox == IntPtr.Zero && what != changes.Add && what != changes.Remove)
|
||||
if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -178,7 +178,9 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
public changes what;
|
||||
public Object arg;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
public class OdeScene : PhysicsScene
|
||||
{
|
||||
private readonly ILog m_log;
|
||||
|
@ -301,6 +303,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
public IntPtr StaticSpace; // space for the static things around
|
||||
public IntPtr GroundSpace; // space for ground
|
||||
|
||||
public IntPtr SharedRay;
|
||||
|
||||
// some speedup variables
|
||||
private int spaceGridMaxX;
|
||||
private int spaceGridMaxY;
|
||||
|
@ -428,6 +432,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
contactgroup = d.JointGroupCreate(0);
|
||||
//contactgroup
|
||||
|
||||
SharedRay = d.CreateRay(TopSpace, 1.0f);
|
||||
|
||||
d.WorldSetAutoDisableFlag(world, false);
|
||||
}
|
||||
}
|
||||
|
@ -733,35 +739,35 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
|
||||
return;
|
||||
/*
|
||||
// debug
|
||||
PhysicsActor dp2;
|
||||
if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
|
||||
{
|
||||
d.AABB aabb;
|
||||
d.GeomGetAABB(g2, out aabb);
|
||||
float x = aabb.MaxX - aabb.MinX;
|
||||
float y = aabb.MaxY - aabb.MinY;
|
||||
float z = aabb.MaxZ - aabb.MinZ;
|
||||
if (x > 60.0f || y > 60.0f || z > 60.0f)
|
||||
{
|
||||
if (!actor_name_map.TryGetValue(g2, out dp2))
|
||||
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
|
||||
else
|
||||
m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
|
||||
dp2.Name, dp2.Size, x, y, z,
|
||||
dp2.Position.ToString(),
|
||||
dp2.Orientation.ToString(),
|
||||
dp2.Orientation.Length());
|
||||
return;
|
||||
}
|
||||
}
|
||||
//
|
||||
*/
|
||||
/*
|
||||
// debug
|
||||
PhysicsActor dp2;
|
||||
if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
|
||||
{
|
||||
d.AABB aabb;
|
||||
d.GeomGetAABB(g2, out aabb);
|
||||
float x = aabb.MaxX - aabb.MinX;
|
||||
float y = aabb.MaxY - aabb.MinY;
|
||||
float z = aabb.MaxZ - aabb.MinZ;
|
||||
if (x > 60.0f || y > 60.0f || z > 60.0f)
|
||||
{
|
||||
if (!actor_name_map.TryGetValue(g2, out dp2))
|
||||
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
|
||||
else
|
||||
m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
|
||||
dp2.Name, dp2.Size, x, y, z,
|
||||
dp2.Position.ToString(),
|
||||
dp2.Orientation.ToString(),
|
||||
dp2.Orientation.Length());
|
||||
return;
|
||||
}
|
||||
}
|
||||
//
|
||||
*/
|
||||
|
||||
|
||||
if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
|
||||
d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc)
|
||||
if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
|
||||
d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
|
||||
{
|
||||
int cflags;
|
||||
unchecked
|
||||
|
@ -776,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
catch (SEHException)
|
||||
{
|
||||
m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
|
||||
// ode.drelease(world);
|
||||
// ode.drelease(world);
|
||||
base.TriggerPhysicsBasedRestart();
|
||||
}
|
||||
catch (Exception e)
|
||||
|
@ -816,26 +822,25 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
// get first contact
|
||||
d.ContactGeom curContact = new d.ContactGeom();
|
||||
|
||||
if (!GetCurContactGeom(0, ref curContact))
|
||||
return;
|
||||
// for now it's the one with max depth
|
||||
ContactPoint maxDepthContact = new ContactPoint(
|
||||
new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
|
||||
new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
|
||||
curContact.depth
|
||||
);
|
||||
|
||||
// do volume detection case
|
||||
if (
|
||||
(p1.IsVolumeDtc || p2.IsVolumeDtc))
|
||||
if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
|
||||
{
|
||||
ContactPoint maxDepthContact = new ContactPoint(
|
||||
new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
|
||||
new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
|
||||
curContact.depth, false
|
||||
);
|
||||
|
||||
collision_accounting_events(p1, p2, maxDepthContact);
|
||||
return;
|
||||
}
|
||||
|
||||
// big messy collision analises
|
||||
|
||||
Vector3 normoverride = Vector3.Zero; //damm c#
|
||||
|
||||
float mu = 0;
|
||||
float bounce = 0;
|
||||
float cfm = 0.0001f;
|
||||
|
@ -846,36 +851,15 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
ContactData contactdata1 = new ContactData(0, 0, false);
|
||||
ContactData contactdata2 = new ContactData(0, 0, false);
|
||||
|
||||
bool dop1foot = false;
|
||||
bool dop2foot = false;
|
||||
bool dop1ava = false;
|
||||
bool dop2ava = false;
|
||||
bool ignore = false;
|
||||
bool AvanormOverride = false;
|
||||
|
||||
switch (p1.PhysicsActorType)
|
||||
{
|
||||
case (int)ActorTypes.Agent:
|
||||
{
|
||||
dop1foot = true;
|
||||
|
||||
AvanormOverride = true;
|
||||
Vector3 tmp = p2.Position - p1.Position;
|
||||
normoverride = p2.Velocity - p1.Velocity;
|
||||
mu = normoverride.LengthSquared();
|
||||
|
||||
if (mu > 1e-6)
|
||||
{
|
||||
mu = 1.0f / (float)Math.Sqrt(mu);
|
||||
normoverride *= mu;
|
||||
mu = Vector3.Dot(tmp, normoverride);
|
||||
if (mu > 0)
|
||||
normoverride *= -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp.Normalize();
|
||||
normoverride = -tmp;
|
||||
}
|
||||
|
||||
dop1ava = true;
|
||||
switch (p2.PhysicsActorType)
|
||||
{
|
||||
case (int)ActorTypes.Agent:
|
||||
|
@ -886,7 +870,6 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
case (int)ActorTypes.Prim:
|
||||
if (p2.Velocity.LengthSquared() > 0.0f)
|
||||
p2.CollidingObj = true;
|
||||
dop1foot = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
@ -901,33 +884,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
{
|
||||
case (int)ActorTypes.Agent:
|
||||
|
||||
dop2ava = true;
|
||||
|
||||
dop2foot = true;
|
||||
|
||||
AvanormOverride = true;
|
||||
|
||||
Vector3 tmp = p2.Position - p1.Position;
|
||||
normoverride = p2.Velocity - p1.Velocity;
|
||||
mu = normoverride.LengthSquared();
|
||||
if (mu > 1e-6)
|
||||
{
|
||||
mu = 1.0f / (float)Math.Sqrt(mu);
|
||||
normoverride *= mu;
|
||||
mu = Vector3.Dot(tmp, normoverride);
|
||||
if (mu > 0)
|
||||
normoverride *= -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp.Normalize();
|
||||
normoverride = -tmp;
|
||||
}
|
||||
|
||||
bounce = 0;
|
||||
mu = 0;
|
||||
cfm = 0.0001f;
|
||||
|
||||
dop2foot = true;
|
||||
if (p1.Velocity.LengthSquared() > 0.0f)
|
||||
p1.CollidingObj = true;
|
||||
break;
|
||||
|
@ -1032,170 +990,78 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (ignore)
|
||||
return;
|
||||
|
||||
|
||||
d.ContactGeom maxContact = curContact;
|
||||
// if (IgnoreNegSides && curContact.side1 < 0)
|
||||
// maxContact.depth = float.MinValue;
|
||||
|
||||
d.ContactGeom minContact = curContact;
|
||||
// if (IgnoreNegSides && curContact.side1 < 0)
|
||||
// minContact.depth = float.MaxValue;
|
||||
|
||||
IntPtr Joint;
|
||||
|
||||
bool FeetCollision = false;
|
||||
|
||||
int i = 0;
|
||||
int ncontacts = 0;
|
||||
while(true)
|
||||
{
|
||||
|
||||
if (IgnoreNegSides && curContact.side1 < 0)
|
||||
|
||||
int i = 0;
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (m_global_contactcount >= maxContactsbeforedeath)
|
||||
break;
|
||||
|
||||
// if (!(IgnoreNegSides && curContact.side1 < 0))
|
||||
{
|
||||
bool noskip = true;
|
||||
if (dop1ava)
|
||||
{
|
||||
if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision)))
|
||||
|
||||
noskip = false;
|
||||
}
|
||||
else if (dop2ava)
|
||||
{
|
||||
if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision)))
|
||||
noskip = false;
|
||||
}
|
||||
|
||||
if (noskip)
|
||||
{
|
||||
m_global_contactcount++;
|
||||
ncontacts++;
|
||||
|
||||
Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
|
||||
d.JointAttach(Joint, b1, b2);
|
||||
|
||||
if (curContact.depth > maxContact.depth)
|
||||
maxContact = curContact;
|
||||
|
||||
if (curContact.depth < minContact.depth)
|
||||
minContact = curContact;
|
||||
}
|
||||
}
|
||||
|
||||
if (++i >= count)
|
||||
break;
|
||||
|
||||
if (!GetCurContactGeom(i, ref curContact))
|
||||
break;
|
||||
}
|
||||
else
|
||||
|
||||
{
|
||||
if(dop1foot)
|
||||
{
|
||||
if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision)))
|
||||
{
|
||||
if (++i >= count)
|
||||
break;
|
||||
else
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if(dop2foot)
|
||||
{
|
||||
if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision)))
|
||||
{
|
||||
if (++i >= count)
|
||||
break;
|
||||
else
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
if (AvanormOverride)
|
||||
{
|
||||
if (curContact.depth > 0.3f)
|
||||
{
|
||||
if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
|
||||
p1.IsColliding = true;
|
||||
if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
|
||||
p2.IsColliding = true;
|
||||
curContact.normal.X = normoverride.X;
|
||||
curContact.normal.Y = normoverride.Y;
|
||||
curContact.normal.Z = normoverride.Z;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
if (dop1foot)
|
||||
{
|
||||
float sz = p1.Size.Z;
|
||||
Vector3 vtmp = p1.Position;
|
||||
float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
|
||||
if (ppos > 0f)
|
||||
{
|
||||
if (!p1.Flying)
|
||||
{
|
||||
d.AABB aabb;
|
||||
d.GeomGetAABB(g2, out aabb);
|
||||
float tmp = vtmp.Z - sz * .18f;
|
||||
|
||||
if (aabb.MaxZ < tmp)
|
||||
{
|
||||
vtmp.X = curContact.pos.X - vtmp.X;
|
||||
vtmp.Y = curContact.pos.Y - vtmp.Y;
|
||||
vtmp.Z = -0.2f;
|
||||
vtmp.Normalize();
|
||||
curContact.normal.X = vtmp.X;
|
||||
curContact.normal.Y = vtmp.Y;
|
||||
curContact.normal.Z = vtmp.Z;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
p1.IsColliding = true;
|
||||
|
||||
}
|
||||
|
||||
if (dop2foot)
|
||||
{
|
||||
float sz = p2.Size.Z;
|
||||
Vector3 vtmp = p2.Position;
|
||||
vtmp.Z -= sz * 0.5f;
|
||||
vtmp.Z += 0.5f;
|
||||
float ppos = vtmp.Z - curContact.pos.Z;
|
||||
if (ppos > 0f)
|
||||
{
|
||||
if (!p2.Flying)
|
||||
{
|
||||
float tmp = vtmp.Z - sz * .18f;
|
||||
vtmp.X = curContact.pos.X - vtmp.X;
|
||||
vtmp.Y = curContact.pos.Y - vtmp.Y;
|
||||
vtmp.Z = curContact.pos.Z - vtmp.Z;
|
||||
vtmp.Normalize();
|
||||
curContact.normal.X = vtmp.X;
|
||||
curContact.normal.Y = vtmp.Y;
|
||||
curContact.normal.Z = vtmp.Z;
|
||||
}
|
||||
}
|
||||
// else
|
||||
p2.IsColliding = true;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
ncontacts++;
|
||||
Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
|
||||
d.JointAttach(Joint, b1, b2);
|
||||
|
||||
if (++m_global_contactcount >= maxContactsbeforedeath)
|
||||
break;
|
||||
|
||||
if (++i >= count)
|
||||
break;
|
||||
|
||||
if (!GetCurContactGeom(i, ref curContact))
|
||||
break;
|
||||
|
||||
if (curContact.depth > maxDepthContact.PenetrationDepth)
|
||||
{
|
||||
maxDepthContact.Position.X = curContact.pos.X;
|
||||
maxDepthContact.Position.Y = curContact.pos.Y;
|
||||
maxDepthContact.Position.Z = curContact.pos.Z;
|
||||
maxDepthContact.SurfaceNormal.X = curContact.normal.X;
|
||||
maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
|
||||
maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
|
||||
maxDepthContact.PenetrationDepth = curContact.depth;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (ncontacts > 0)
|
||||
{
|
||||
maxDepthContact.CharacterFeet = FeetCollision;
|
||||
ContactPoint maxDepthContact = new ContactPoint(
|
||||
new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z),
|
||||
new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z),
|
||||
maxContact.depth, FeetCollision
|
||||
);
|
||||
collision_accounting_events(p1, p2, maxDepthContact);
|
||||
}
|
||||
/*
|
||||
if (notskipedcount > geomContactPointsStartthrottle)
|
||||
{
|
||||
// If there are more then 3 contact points, it's likely
|
||||
// that we've got a pile of objects, so ...
|
||||
// We don't want to send out hundreds of terse updates over and over again
|
||||
// so lets throttle them and send them again after it's somewhat sorted out.
|
||||
this needs checking so out for now
|
||||
if (b1 != IntPtr.Zero)
|
||||
p1.ThrottleUpdates = true;
|
||||
if (b2 != IntPtr.Zero)
|
||||
p2.ThrottleUpdates = true;
|
||||
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
|
||||
|
@ -1286,10 +1152,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
// chr.CollidingGround = false; not done here
|
||||
chr.CollidingObj = false;
|
||||
// do colisions with static space
|
||||
d.SpaceCollide2(StaticSpace, chr.topbox, IntPtr.Zero, nearCallback);
|
||||
d.SpaceCollide2(StaticSpace, chr.midbox, IntPtr.Zero, nearCallback);
|
||||
d.SpaceCollide2(StaticSpace, chr.feetbox, IntPtr.Zero, nearCallback);
|
||||
d.SpaceCollide2(StaticSpace, chr.bonebox, IntPtr.Zero, nearCallback);
|
||||
d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
|
||||
|
||||
// chars with chars
|
||||
d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
|
||||
|
@ -1346,7 +1209,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
// and with chars
|
||||
try
|
||||
{
|
||||
d.SpaceCollide2(ActiveSpace, CharsSpace,IntPtr.Zero, nearCallback);
|
||||
d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
|
||||
}
|
||||
catch (AccessViolationException)
|
||||
{
|
||||
|
@ -1837,7 +1700,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
foreach (OdeCharacter actor in _characters)
|
||||
{
|
||||
if (actor != null)
|
||||
actor.Move(ODE_STEPSIZE, defects);
|
||||
actor.Move(defects);
|
||||
}
|
||||
if (defects.Count != 0)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue