private void changeOrientation(Quaternion newOri)
{
if (m_orientation != newOri)
{
m_orientation = newOri; // keep a copy for core use
// but only use rotations around Z
m_orientation2D.W = newOri.W;
m_orientation2D.Z = newOri.Z;
float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
if (t > 0)
{
t = 1.0f / (float)Math.Sqrt(t);
m_orientation2D.W *= t;
m_orientation2D.Z *= t;
}
else
{
m_orientation2D.W = 1.0f;
m_orientation2D.Z = 0f;
}
m_orientation2D.Y = 0f;
m_orientation2D.X = 0f;
d.Quaternion myrot = new d.Quaternion();
myrot.X = m_orientation2D.X;
myrot.Y = m_orientation2D.Y;
myrot.Z = m_orientation2D.Z;
myrot.W = m_orientation2D.W;
d.BodySetQuaternion(Body, ref myrot);
}
}
private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
{
if (m_isphysical)
{
if (childPrim && m_building) // inertia is messed, must rebuild
{
_position = newPos;
_orientation = newOri;
}
else
{
if (newOri != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = newOri.X;
myrot.Y = newOri.Y;
myrot.Z = newOri.Z;
myrot.W = newOri.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newOri;
if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
if (_position != newPos)
{
d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
_position = newPos;
}
if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
d.BodyEnable(Body);
}
}
else
{
// string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
// int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
if (prim_geom != IntPtr.Zero)
{
if (newOri != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = newOri.X;
myrot.Y = newOri.Y;
myrot.Z = newOri.Z;
myrot.W = newOri.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newOri;
}
if (newPos != _position)
{
d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
_position = newPos;
m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
}
}
}
givefakepos--;
if (givefakepos < 0)
givefakepos = 0;
givefakeori--;
if (givefakeori < 0)
givefakeori = 0;
resetCollisionAccounting();
}
private void changeprimsizeshape()
{
_parent_scene.actor_name_map.Remove(prim_geom);
OdePrim parent = (OdePrim)_parent;
bool chp = childPrim;
if (chp)
{
if (parent != null)
{
parent.DestroyBody();
}
}
else
{
DestroyBody();
}
if (prim_geom != IntPtr.Zero)
{
try
{
d.GeomDestroy(prim_geom);
if (_triMeshData != IntPtr.Zero)
{
d.GeomTriMeshDataDestroy(_triMeshData);
_triMeshData = IntPtr.Zero;
}
}
// catch (System.AccessViolationException)
catch (Exception e)
{
m_log.Error("[PHYSICS]: PrimGeom destruction failed: {0}", e);
}
prim_geom = IntPtr.Zero;
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0)
_size.X = 0.01f;
if (_size.Y <= 0)
_size.Y = 0.01f;
if (_size.Z <= 0)
_size.Z = 0.01f;
// Construction of new prim
IMesh mesh = null;
if (_parent_scene.needsMeshing(_pbs))
{
mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true);
}
CreateGeom(m_targetSpace, mesh);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
}
if (chp)
{
if (parent != null)
{
parent.MakeBody();
}
}
else
MakeBody();
resetCollisionAccounting();
}
private void changeadd()
{
IMesh mesh = null;
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in SetMesh
mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true);
// createmesh returns null when it's a shape that isn't a cube.
// m_log.Debug(m_localID);
}
//Console.WriteLine("changeadd 1");
CreateGeom(m_targetSpace, mesh);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
// _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
if (!m_isphysical)
SetInStaticSpace(this);
}
if (m_isphysical && Body == IntPtr.Zero)
{
/*
if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
{
changeShape(_pbs);
}
else
{
*/
MakeBody();
// }
}
}
private void changeOrientation(Quaternion newOri)
{
if (m_isphysical)
{
if (childPrim) // inertia is messed, must rebuild
{
if (m_building)
{
_orientation = newOri;
}
}
else
{
if (newOri != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = newOri.X;
myrot.Y = newOri.Y;
myrot.Z = newOri.Z;
myrot.W = newOri.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newOri;
if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
d.BodyEnable(Body);
}
}
else
{
if (prim_geom != IntPtr.Zero)
{
if (newOri != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = newOri.X;
myrot.Y = newOri.Y;
myrot.Z = newOri.Z;
myrot.W = newOri.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newOri;
}
}
}
givefakeori--;
if (givefakeori < 0)
givefakeori = 0;
resetCollisionAccounting();
}
private void MakeBody()
{
if (!m_isphysical) // only physical get bodies
return;
if (childPrim) // child prims don't get bodies;
return;
if (m_building)
return;
if (prim_geom == IntPtr.Zero)
{
m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
return;
}
if (Body != IntPtr.Zero)
{
d.BodyDestroy(Body);
Body = IntPtr.Zero;
m_log.Warn("[PHYSICS]: MakeBody called having a body");
}
d.Matrix3 mymat = new d.Matrix3();
d.Quaternion myrot = new d.Quaternion();
d.Mass objdmass = new d.Mass { };
Body = d.BodyCreate(_parent_scene.world);
DMassDup(ref primdMass, out objdmass);
// rotate inertia
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.RfromQ(out mymat, ref myrot);
d.MassRotate(ref objdmass, ref mymat);
// set the body rotation and position
d.BodySetRotation(Body, ref mymat);
// recompute full object inertia if needed
if (childrenPrim.Count > 0)
{
d.Matrix3 mat = new d.Matrix3();
d.Quaternion quat = new d.Quaternion();
d.Mass tmpdmass = new d.Mass { };
Vector3 rcm;
rcm.X = _position.X + objdmass.c.X;
rcm.Y = _position.Y + objdmass.c.Y;
rcm.Z = _position.Z + objdmass.c.Z;
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
continue;
}
DMassCopy(ref prm.primdMass, ref tmpdmass);
// apply prim current rotation to inertia
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
quat.W = prm._orientation.W;
d.RfromQ(out mat, ref quat);
d.MassRotate(ref tmpdmass, ref mat);
Vector3 ppos = prm._position;
ppos.X += tmpdmass.c.X - rcm.X;
ppos.Y += tmpdmass.c.Y - rcm.Y;
ppos.Z += tmpdmass.c.Z - rcm.Z;
// refer inertia to root prim center of mass position
d.MassTranslate(ref tmpdmass,
ppos.X,
ppos.Y,
ppos.Z);
d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
// fix prim colision cats
d.GeomClearOffset(prm.prim_geom);
d.GeomSetBody(prm.prim_geom, Body);
prm.Body = Body;
d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
}
}
}
d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
// associate root geom with body
d.GeomSetBody(prim_geom, Body);
d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
myrot.W = -myrot.W;
d.RfromQ(out mymat, ref myrot);
d.MassRotate(ref objdmass, ref mymat);
d.BodySetMass(Body, ref objdmass);
_mass = objdmass.mass;
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
// disconnect from world gravity so we can apply buoyancy
d.BodySetGravityMode(Body, false);
d.BodySetAutoDisableFlag(Body, true);
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
// d.BodySetLinearDampingThreshold(Body, 0.01f);
// d.BodySetAngularDampingThreshold(Body, 0.001f);
d.BodySetDamping(Body, .001f, .0002f);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
m_interpenetrationcount = 0;
m_collisionscore = 0;
m_disabled = false;
if (m_targetSpace != _parent_scene.ActiveSpace)
{
if (m_targetSpace != IntPtr.Zero)
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
if (d.SpaceQuery(m_targetSpace, prim_geom))
d.SpaceRemove(m_targetSpace, prim_geom);
}
m_targetSpace = _parent_scene.ActiveSpace;
d.SpaceAdd(m_targetSpace, prim_geom);
}
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
if (prm.prim_geom == IntPtr.Zero)
continue;
Vector3 ppos = prm._position;
d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
if (prm.m_targetSpace != _parent_scene.ActiveSpace)
{
if (prm.m_targetSpace != IntPtr.Zero)
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
}
prm.m_targetSpace = _parent_scene.ActiveSpace;
d.SpaceAdd(m_targetSpace, prm.prim_geom);
}
prm.m_disabled = false;
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
_parent_scene.addActivePrim(prm);
}
}
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
{
createAMotor(m_angularlock);
}
_parent_scene.addActivePrim(this);
}
private void UpdateDataFromGeom()
{
if (prim_geom != IntPtr.Zero)
{
d.Vector3 lpos = d.GeomGetPosition(prim_geom);
_position.X = lpos.X;
_position.Y = lpos.Y;
_position.Z = lpos.Z;
d.Quaternion qtmp = new d.Quaternion { };
d.GeomCopyQuaternion(prim_geom, out qtmp);
_orientation.W = qtmp.W;
_orientation.X = qtmp.X;
_orientation.Y = qtmp.Y;
_orientation.Z = qtmp.Z;
}
}
public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
{
// assumes 1m size grid and constante size square regions
// needs to know about sims around in future
float[] _heightmap;
uint regionsizeX = m_regionWidth;
uint regionsizeY = m_regionHeight;
// map is rotated
uint heightmapWidth = regionsizeY + 2;
uint heightmapHeight = regionsizeX + 2;
uint heightmapWidthSamples = heightmapWidth + 1;
uint heightmapHeightSamples = heightmapHeight + 1;
_heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
const float scale = 1.0f;
const float offset = 0.0f;
const float thickness = 10f;
const int wrap = 0;
float hfmin = float.MaxValue;
float hfmax = float.MinValue;
float val;
uint xx;
uint yy;
uint maxXX = regionsizeX - 1;
uint maxYY = regionsizeY - 1;
// flipping map adding one margin all around so things don't fall in edges
uint xt = 0;
xx = 0;
for (uint x = 0; x < heightmapWidthSamples; x++)
{
if (x > 1 && xx < maxXX)
xx++;
yy = 0;
for (uint y = 0; y < heightmapHeightSamples; y++)
{
if (y > 1 && y < maxYY)
yy += regionsizeX;
val = heightMap[yy + xx];
if (val < 0.0f)
val = 0.0f; // no neg terrain as in chode
_heightmap[xt + y] = val;
if (hfmin > val)
hfmin = val;
if (hfmax < val)
hfmax = val;
}
xt += heightmapHeightSamples;
}
lock (OdeLock)
{
d.AllocateODEDataForThread(~0U);
IntPtr GroundGeom = IntPtr.Zero;
if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
{
RegionTerrain.Remove(pOffset);
if (GroundGeom != IntPtr.Zero)
{
actor_name_map.Remove(GroundGeom);
d.GeomDestroy(GroundGeom);
if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
{
TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
TerrainHeightFieldHeights.Remove(GroundGeom);
}
}
}
IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0,
heightmapHeight, heightmapWidth ,
(int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
offset, thickness, wrap);
d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
if (GroundGeom != IntPtr.Zero)
{
d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
d.GeomSetCollideBits(GroundGeom, 0);
PhysicsActor pa = new NullPhysicsActor();
pa.Name = "Terrain";
pa.PhysicsActorType = (int)ActorTypes.Ground;
actor_name_map[GroundGeom] = pa;
// geom_name_map[GroundGeom] = "Terrain";
d.Quaternion q = new d.Quaternion();
q.X = 0.5f;
q.Y = 0.5f;
q.Z = 0.5f;
q.W = 0.5f;
d.GeomSetQuaternion(GroundGeom, ref q);
d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0.0f);
RegionTerrain.Add(pOffset, GroundGeom);
TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
}
}
}
} //end MoveAngular
internal void LimitRotation(float timestep, IntPtr Body)
{
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
d.Quaternion m_rot = new d.Quaternion();
bool changed = false;
m_rot.X = rotq.X;
m_rot.Y = rotq.Y;
m_rot.Z = rotq.Z;
m_rot.W = rotq.W;
if (m_RollreferenceFrame != Quaternion.Identity)
{
if (rotq.X >= m_RollreferenceFrame.X)
{
m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
}
if (rotq.Y >= m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
}
if (rotq.X <= -m_RollreferenceFrame.X)
{
m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
}
if (rotq.Y <= -m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
}
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
m_rot.X = 0;
m_rot.Y = 0;
changed = true;
}
if (changed)
d.BodySetQuaternion(Body, ref m_rot);
}
