/// <summary>
/// Add a child prim to this parent prim.
/// </summary>
/// <param name="prim">Child prim</param>
private void AddChildPrim(OdePrim prim)
{
if (LocalID == prim.LocalID)
return;
if (Body == IntPtr.Zero)
{
Body = d.BodyCreate(_parent_scene.world);
setMass();
}
lock (childrenPrim)
{
if (childrenPrim.Contains(prim))
return;
// m_log.DebugFormat(
// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
childrenPrim.Add(prim);
foreach (OdePrim prm in childrenPrim)
{
d.Mass m2;
d.MassSetZero(out m2);
d.MassSetBoxTotal(out m2, prm.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
d.Quaternion quat = new d.Quaternion();
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.Matrix3 mat = new d.Matrix3();
d.RfromQ(out mat, ref quat);
d.MassRotate(ref m2, ref mat);
d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
d.MassAdd(ref pMass, ref m2);
}
foreach (OdePrim prm in childrenPrim)
{
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
if (prm.m_assetFailed)
{
d.GeomSetCategoryBits(prm.prim_geom, 0);
d.GeomSetCollideBits(prm.prim_geom, (uint)prm.BadMeshAssetCollideBits);
}
else
{
d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
}
d.Quaternion quat = new d.Quaternion();
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.Matrix3 mat = new d.Matrix3();
d.RfromQ(out mat, ref quat);
if (Body != IntPtr.Zero)
{
d.GeomSetBody(prm.prim_geom, Body);
prm.childPrim = true;
d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
//d.GeomSetOffsetPosition(prim.prim_geom,
// (Position.X - prm.Position.X) - pMass.c.X,
// (Position.Y - prm.Position.Y) - pMass.c.Y,
// (Position.Z - prm.Position.Z) - pMass.c.Z);
d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
//d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
d.BodySetMass(Body, ref pMass);
}
else
{
m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
}
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
prm.m_disabled = false;
prm.Body = Body;
_parent_scene.ActivatePrim(prm);
}
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
if (m_assetFailed)
{
d.GeomSetCategoryBits(prim_geom, 0);
d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
}
else
{
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
}
d.Quaternion quat2 = new d.Quaternion();
quat2.W = _orientation.W;
quat2.X = _orientation.X;
quat2.Y = _orientation.Y;
quat2.Z = _orientation.Z;
d.Matrix3 mat2 = new d.Matrix3();
d.RfromQ(out mat2, ref quat2);
d.GeomSetBody(prim_geom, Body);
d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
//d.GeomSetOffsetPosition(prim.prim_geom,
// (Position.X - prm.Position.X) - pMass.c.X,
// (Position.Y - prm.Position.Y) - pMass.c.Y,
// (Position.Z - prm.Position.Z) - pMass.c.Z);
//d.GeomSetOffsetRotation(prim_geom, ref mat2);
d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
d.BodySetMass(Body, ref pMass);
d.BodySetAutoDisableFlag(Body, true);
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
m_interpenetrationcount = 0;
m_collisionscore = 0;
m_disabled = false;
// The body doesn't already have a finite rotation mode set here
// or remove
if (_parent == null)
{
createAMotor(m_angularlock);
}
d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
if (m_vehicle.Type != Vehicle.TYPE_NONE)
m_vehicle.Enable(Body, _parent_scene);
_parent_scene.ActivatePrim(this);
}
}
private void SetTerrain(float[] heightMap, Vector3 pOffset)
{
int startTime = Util.EnvironmentTickCount();
m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", PhysicsSceneName, pOffset);
float[] _heightmap;
// ok im lasy this are just a aliases
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;
_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)
{
if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
{
TerrainHeightFieldHeights.Remove(GroundGeom);
}
d.SpaceRemove(space, GroundGeom);
d.GeomDestroy(GroundGeom);
}
}
IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
heightmapWidth, heightmapHeight,
(int)heightmapWidthSamples,
(int)heightmapHeightSamples,
scale, offset, thickness, wrap);
d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
if (GroundGeom != IntPtr.Zero)
{
d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
}
geom_name_map[GroundGeom] = "Terrain";
d.Matrix3 R = new d.Matrix3();
Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
q1 = q1 * q2;
Vector3 v3;
float angle;
q1.GetAxisAngle(out v3, out angle);
d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
d.GeomSetRotation(GroundGeom, ref R);
d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0.0f);
IntPtr testGround = IntPtr.Zero;
if (RegionTerrain.TryGetValue(pOffset, out testGround))
{
RegionTerrain.Remove(pOffset);
}
RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
}
m_log.DebugFormat(
"[ODE SCENE]: Setting terrain for {0} took {1}ms", PhysicsSceneName, Util.EnvironmentTickCountSubtract(startTime));
}