public void MakeBody ()
{
// d.Vector3 dvtmp;
// d.Vector3 dbtmp;
d.Mass tmpdmass = new d.Mass
{
};
d.Mass objdmass = new d.Mass
{
};
d.Matrix3 mat = new d.Matrix3 ();
d.Matrix3 mymat = new d.Matrix3 ();
d.Quaternion quat = new d.Quaternion ();
d.Quaternion myrot = new d.Quaternion ();
Vector3 rcm;
if (childPrim) // child prims don't get own bodies;
return;
if (Body != IntPtr.Zero) // who shouldn't have one already ?
{
d.BodyDestroy (Body);
Body = IntPtr.Zero;
}
if (!m_isphysical) // only physical things get a body
return;
Body = d.BodyCreate (_parent_scene.world);
calcdMass (); // compute inertia on local frame
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)
{
rcm.X = _position.X + objdmass.c.X;
rcm.Y = _position.Y + objdmass.c.Y;
rcm.Z = _position.Z + objdmass.c.Z;
lock (childrenPrim)
{
foreach (AuroraODEPrim prm in childrenPrim)
{
prm.calcdMass (); // recompute inertia on local frame
DMassCopy (ref prm.primdMass, ref tmpdmass);
// apply prim current rotation to inertia
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
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
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn ("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
continue;
}
d.GeomClearOffset (prm.prim_geom);
d.GeomSetBody (prm.prim_geom, 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
if (!testRealGravity)
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, .001f);
m_disabled = false;
d.GeomSetCategoryBits (prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits (prim_geom, (int)m_collisionFlags);
m_interpenetrationcount = 0;
m_collisionscore = 0;
if (m_targetSpace != _parent_scene.space)
{
_parent_scene.waitForSpaceUnlock (m_targetSpace);
if (d.SpaceQuery (m_targetSpace, prim_geom))
d.SpaceRemove (m_targetSpace, prim_geom);
m_targetSpace = _parent_scene.space;
d.SpaceAdd (m_targetSpace, prim_geom);
}
lock (childrenPrim)
{
foreach (AuroraODEPrim prm in childrenPrim)
{
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn ("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
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);
prm.Body = Body;
prm.m_disabled = false;
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
_parent_scene.addActivePrim (prm);
if (prm.m_targetSpace != _parent_scene.space)
{
_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.space;
d.SpaceAdd (m_targetSpace, prm.prim_geom);
}
}
}
// 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);
}
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
m_vehicle.Enable (Body, this, _parent_scene);
}
_parent_scene.addActivePrim (this);
/* d.Mass mtmp;
d.BodyGetMass(Body, out mtmp);
d.Matrix3 mt = d.GeomGetRotation(prim_geom);
d.Matrix3 mt2 = d.BodyGetRotation(Body);
dvtmp = d.GeomGetPosition(prim_geom);
dbtmp = d.BodyGetPosition(Body);
*/
}
// I'm the parent
// prim is the child
public void ParentPrim(AuroraODEPrim prim)
{
//Console.WriteLine("ParentPrim " + m_primName);
if (this.m_localID != prim.m_localID)
{
if (Body == IntPtr.Zero)
{
Body = d.BodyCreate(_parent_scene.world);
setMass();
}
if (Body != IntPtr.Zero)
{
lock (childrenPrim)
{
if (!childrenPrim.Contains(prim))
{
//Console.WriteLine("childrenPrim.Add " + prim);
childrenPrim.Add(prim);
foreach (AuroraODEPrim prm in childrenPrim)
{
d.Mass m2;
d.MassSetZero(out m2);
prim.Mass = prim.CalculateMass();
d.MassSetBoxTotal(out m2, prim.Mass, 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 (AuroraODEPrim prm in childrenPrim)
{
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
continue;
}
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int)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.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
}
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
prm.m_disabled = false;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
{
prm.createAMotor(m_angularlock);
}
prm.Body = Body;
_parent_scene.addActivePrim(prm);
}
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
d.GeomSetCollideBits(prim_geom, (int)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
if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _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, this, _parent_scene);
_parent_scene.addActivePrim(this);
}
}
}
}
}
private void createAMotor(Vector3 axis)
{
if (Body == IntPtr.Zero)
return;
if (Amotor != IntPtr.Zero)
{
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
float axisnum = 3;
axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
// PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
// Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
d.Mass objMass;
d.MassSetZero(out objMass);
DMassCopy(ref pMass, ref objMass);
//m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
Matrix4 dMassMat = FromDMass(objMass);
Matrix4 mathmat = Inverse(dMassMat);
/*
//m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
mathmat = Inverse(mathmat);
objMass = FromMatrix4(mathmat, ref objMass);
//m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
mathmat = Inverse(mathmat);
*/
if (axis.X == 0)
{
mathmat.M33 = 50.0000001f;
//objMass.I.M22 = 0;
}
if (axis.Y == 0)
{
mathmat.M22 = 50.0000001f;
//objMass.I.M11 = 0;
}
if (axis.Z == 0)
{
mathmat.M11 = 50.0000001f;
//objMass.I.M00 = 0;
}
mathmat = Inverse(mathmat);
objMass = FromMatrix4(mathmat, ref objMass);
//m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
//return;
if (d.MassCheck(ref objMass))
{
d.BodySetMass(Body, ref objMass);
}
else
{
//m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
}
if (axisnum <= 0)
return;
// int dAMotorEuler = 1;
Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
d.JointAttach(Amotor, Body, IntPtr.Zero);
int dAMotorEuler = 1;
d.JointSetAMotorMode(Amotor, dAMotorEuler);
d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
int i = 0;
if (axis.X == 0)
{
d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
i++;
}
if (axis.Y == 0)
{
d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
i++;
}
if (axis.Z == 0)
{
d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
i++;
}
/*for (int j = 0; j < (int)axisnum; j++)
{
d.JointSetAMotorAngle(Amotor, j, 0);
}*/
// These lowstops and high stops are effectively (no wiggle room)
d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.01f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0.01f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.01f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.01f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.01f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.01f);
d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 9000);
d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 55000000);
d.JointAttach(Amotor, Body, IntPtr.Zero);
d.JointAddAMotorTorques(Amotor, 10000, 10000, 10000);
AmotorRotation = d.BodyGetRotation(Body);
}
/*public void SetTerrain(float[] heightMap, Vector3 pOffset)
{
// this._heightmap[i] = (double)heightMap[i];
// dbm (danx0r) -- creating a buffer zone of one extra sample all around
//_origheightmap = heightMap;
float[] _heightmap;
// zero out a heightmap array float array (single dimension [flattened]))
//if ((int)Constants.RegionSize == 256)
// _heightmap = new float[514 * 514];
//else
_heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
uint heightmapWidth = Constants.RegionSize + 1;
uint heightmapHeight = Constants.RegionSize + 1;
uint heightmapWidthSamples;
uint heightmapHeightSamples;
//if (((int)Constants.RegionSize) == 256)
//{
// heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
// heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
// heightmapWidth++;
// heightmapHeight++;
//}
//else
//{
heightmapWidthSamples = (uint)Constants.RegionSize + 1;
heightmapHeightSamples = (uint)Constants.RegionSize + 1;
//}
const float scale = 1.0f;
const float offset = 0.0f;
const float thickness = 1.0f;
const int wrap = 0;
int regionsize = (int) Constants.RegionSize + 2;
//Double resolution
//if (((int)Constants.RegionSize) == 256)
// heightMap = ResizeTerrain512Interpolation(heightMap);
// if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256)
// regionsize = 512;
float hfmin = 2000;
float hfmax = -2000;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
int xx = Util.Clip(x - 1, 0, regionsize - 1);
int yy = Util.Clip(y - 1, 0, regionsize - 1);
float val= heightMap[yy * (int)Constants.RegionSize + xx];
_heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
lock (OdeLock)
{
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 + 1, heightmapHeight + 1,
(int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, 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);
//Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
q1 = q1 * q2;
//q1 = q1 * q3;
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 + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0);
IntPtr testGround = IntPtr.Zero;
if (RegionTerrain.TryGetValue(pOffset, out testGround))
{
RegionTerrain.Remove(pOffset);
}
RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
}
}*/
public void SetTerrain(float[] heightMap, double[,] normalHeightMap, Vector3 pOffset)
{
// double[] _heightmap = new double[(((int)Math.Sqrt(heightMap.Length) + 2) * ((int)Math.Sqrt(heightMap.Length) + 2))];
// double[] _heightmap = new double[((m_region.RegionSizeX + 2) * (m_region.RegionSizeY + 2))];
float[] _heightmap = new float[((m_region.RegionSizeX + 2) * (m_region.RegionSizeY + 2))];
int heightmapWidth = m_region.RegionSizeX + 1;
int heightmapHeight = m_region.RegionSizeY + 1;
int heightmapWidthSamples = m_region.RegionSizeX + 2;
int heightmapHeightSamples = m_region.RegionSizeY + 2;
#pragma warning disable 0162
/*
if (Constants.RegionSize == 256 &&
m_region.RegionSizeX == Constants.RegionSize && m_region.RegionSizeY == Constants.RegionSize)
{
// -- creating a buffer zone of one extra sample all around - danzor
heightmapWidthSamples = 2 * Constants.RegionSize + 2;
heightmapHeightSamples = 2 * Constants.RegionSize + 2;
heightmapWidth++;
heightmapHeight++;
}
*/
#pragma warning restore 0162
/* no resolution duplication for now
int regionsize = (int)Constants.RegionSize;
double hfmin = 2000;
double hfmax = -2000;
if (regionsize == 256 &&
m_region.RegionSizeX == Constants.RegionSize && m_region.RegionSizeY == Constants.RegionSize)
{
//Double resolution
_heightmap = new double[((((int)Constants.RegionSize * 2) + 2) * (((int)Constants.RegionSize * 2) + 2))];
heightMap = ResizeTerrain512Interpolation(heightMap);
regionsize *= 2;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
int xx = Util.Clip(x - 1, 0, (regionsize - 1) - 1);
int yy = Util.Clip(y - 1, 0, (regionsize - 1) - 1);
float val = heightMap[yy * regionsize + xx];
_heightmap[x * heightmapWidthSamples + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
}
else
{
*/
// int rSize = sqrtOfHeightMap > Constants.RegionSize ? sqrtOfHeightMap : Constants.RegionSize;
// heightmapWidth = rSize + 2;
// heightmapHeight = rSize + 2;
// heightmapWidthSamples = rSize + 2;
// heightmapHeightSamples = rSize + 2;
// _heightmap = new double[heightmapWidthSamples * heightmapHeightSamples];
// double hfmin = 2000;
// double hfmax = -2000;
float hfmin = 2000;
float hfmax = -2000;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
//Some notes on this part
//xx and yy are used for the original heightmap, as we are offsetting the new one by 1
// so we subtract one so that we can put the heightmap in correctly
int xx = Util.Clip(x - 1, 0, m_region.RegionSizeX - 1);
int yy = Util.Clip(y - 1, 0, m_region.RegionSizeY - 1);
float val = heightMap[yy * m_region.RegionSizeX + xx];
//ODE is evil... flip x and y
_heightmap[(x * heightmapHeightSamples) + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
// }
lock (OdeLock)
{
IntPtr GroundGeom = IntPtr.Zero;
if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
{
if (GroundGeom != IntPtr.Zero)
{
d.SpaceRemove(space, GroundGeom);
d.GeomDestroy(GroundGeom);
}
RegionTerrain.Remove(pOffset);
TerrainHeightFieldHeights.Remove(GroundGeom);
TerrainHeightFieldlimits.Remove(GroundGeom);
actor_name_map.Remove(GroundGeom);
geom_name_map.Remove(GroundGeom);
}
const float scale = 1.0f;
const float offset = 0.0f;
float thickness = (float)hfmin - 1.0f ;
const int wrap = 0;
IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
// d.GeomHeightfieldDataBuildDouble(HeightmapData, _heightmap, 1, heightmapWidth, heightmapHeight,
// heightmapWidthSamples, heightmapHeightSamples, scale,
d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 1, heightmapHeight, heightmapWidth,
heightmapHeightSamples, heightmapWidthSamples, scale,
offset, thickness, wrap);
// d.GeomHeightfieldDataSetBounds(HeightmapData, (float)hfmin - 1.0f, (float)hfmax + 1.0f);
d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin, (float)hfmax + 1.0f);
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";
NullPhysicsActor terrainActor = new NullPhysicsActor()
{
PhysicsActorType = (int)ActorTypes.Ground
};
actor_name_map[GroundGeom] = terrainActor;
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 + (m_region.RegionSizeX * 0.5f)), (pOffset.Y + (m_region.RegionSizeY * 0.5f)), 0);
RegionTerrain.Remove(pOffset);
RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
// TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
float[] heighlimits = new float[2];
heighlimits[0] = hfmin;
heighlimits[1] = hfmax;
TerrainHeightFieldHeights.Add(GroundGeom, heightMap);
TerrainHeightFieldlimits.Add(GroundGeom, heighlimits);
}
}
public override void SetTerrain(ITerrainChannel channel, short[] heightMap)
{
m_channel = channel;
bool needToCreateHeightmapinODE = false;
short[] _heightmap = ODETerrainHeightFieldHeights;
if (ODETerrainHeightFieldHeights == null)
{
needToCreateHeightmapinODE = true; //We don't have any terrain yet, we need to generate one
_heightmap = new short[((m_region.RegionSizeX + 3)*(m_region.RegionSizeY + 3))];
}
int heightmapWidth = m_region.RegionSizeX + 2;
int heightmapHeight = m_region.RegionSizeY + 2;
int heightmapWidthSamples = m_region.RegionSizeX + 3; // + one to complete the 256m + 2 margins each side
int heightmapHeightSamples = m_region.RegionSizeY + 3;
float hfmin = 2000;
float hfmax = -2000;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
//Some notes on this part
//xx and yy are used for the original heightmap, as we are offsetting the new one by 1
// so we subtract one so that we can put the heightmap in correctly
int xx = x - 1;
if (xx < 0)
xx = 0;
if (xx > m_region.RegionSizeX - 1)
xx = m_region.RegionSizeX - 1;
int yy = y - 1;
if (yy < 0)
yy = 0;
if (yy > m_region.RegionSizeY - 1)
yy = m_region.RegionSizeY - 1;
short val = heightMap[yy*m_region.RegionSizeX + xx];
//ODE is evil... flip x and y
_heightmap[(x*heightmapHeightSamples) + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
needToCreateHeightmapinODE = true; //ODE seems to have issues with not rebuilding :(
if (RegionTerrain != IntPtr.Zero)
{
d.SpaceRemove(space, RegionTerrain);
d.GeomDestroy(RegionTerrain);
}
if (!needToCreateHeightmapinODE)
{
TerrainHeightFieldHeights = null;
TerrainHeightFieldlimits = null;
ODETerrainHeightFieldHeights = null;
float[] heighlimits = new float[2];
heighlimits[0] = hfmin;
heighlimits[1] = hfmax;
TerrainHeightFieldHeights = heightMap;
TerrainHeightFieldlimits = heighlimits;
ODETerrainHeightFieldHeights = _heightmap;
return; //If we have already done this once, we don't need to do it again
}
lock (OdeLock)
{
const float scale = (1f/Constants.TerrainCompression);
const float offset = 0.0f;
float thickness = 0.01f;
const int wrap = 0;
IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
GC.AddMemoryPressure(_heightmap.Length);
//Add the memory pressure properly (note: should we be doing this since we have it in managed memory?)
//Do NOT copy it! Otherwise, it'll copy the terrain into unmanaged memory where we can't release it each time
d.GeomHeightfieldDataBuildShort(HeightmapData, _heightmap, 0, heightmapHeight, heightmapWidth,
heightmapHeightSamples, heightmapWidthSamples, scale,
offset, thickness, wrap);
d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1.0f, hfmax + 1.0f);
RegionTerrain = d.CreateHeightfield(space, HeightmapData, 1);
if (RegionTerrain != IntPtr.Zero)
{
d.GeomSetCategoryBits(RegionTerrain, (int) (CollisionCategories.Land));
d.GeomSetCollideBits(RegionTerrain, (int) (CollisionCategories.Space));
}
NullObjectPhysicsActor terrainActor = new NullObjectPhysicsActor();
actor_name_map[RegionTerrain] = terrainActor;
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(RegionTerrain, ref R);
d.GeomSetPosition(RegionTerrain, (m_region.RegionSizeX*0.5f), (m_region.RegionSizeY*0.5f), 0);
float[] heighlimits = new float[2];
heighlimits[0] = hfmin;
heighlimits[1] = hfmax;
TerrainHeightFieldHeights = heightMap;
ODETerrainHeightFieldHeights = _heightmap;
TerrainHeightFieldlimits = heighlimits;
}
}
public override void SetTerrain(ITerrainChannel channel, short[] heightMap)
{
m_channel = channel;
float[] _heightmap = ODETerrainHeightFieldHeights;
if (ODETerrainHeightFieldHeights == null)
_heightmap = new float[m_region.RegionSizeX*m_region.RegionSizeY];
for (int x = 0; x < m_region.RegionSizeX; x++)
{
for (int y = 0; y < m_region.RegionSizeY; y++)
{
_heightmap[(x*m_region.RegionSizeX) + y] = heightMap[y*m_region.RegionSizeX + x]/
Constants.TerrainCompression;
}
}
float hfmin = _heightmap.Min();
float hfmax = _heightmap.Max();
SimulationChangesQueue.Enqueue(() =>
{
if (RegionTerrain != IntPtr.Zero)
{
d.GeomHeightfieldDataDestroy(RegionTerrain);
d.SpaceRemove(space, RegionTerrain);
//d.GeomDestroy(RegionTerrain);
GC.RemoveMemoryPressure(_heightmap.Length);
}
const float scale = 1f;
const float offset = 0.0f;
float thickness = 0.2f;
const int wrap = 0;
IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
GC.AddMemoryPressure(_heightmap.Length);
//Add the memory pressure properly (note: should we be doing this since we have it in managed memory?)
//Do NOT copy it! Otherwise, it'll copy the terrain into unmanaged memory where we can't release it each time
d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
m_region.RegionSizeX,
m_region.RegionSizeY,
m_region.RegionSizeX,
m_region.RegionSizeY, scale,
offset, thickness, wrap);
d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1.0f,
hfmax + 1.0f);
RegionTerrain = d.CreateHeightfield(space, HeightmapData, 1);
d.GeomSetCategoryBits(RegionTerrain, (int) (CollisionCategories.Land));
d.GeomSetCollideBits(RegionTerrain, (int) (CollisionCategories.Space));
actor_name_map[RegionTerrain] = new NullObjectPhysicsActor();
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(RegionTerrain, ref R);
d.GeomSetPosition(RegionTerrain, (m_region.RegionSizeX*0.5f),
(m_region.RegionSizeY*0.5f), 0);
TerrainHeightFieldHeights = heightMap;
ODETerrainHeightFieldHeights = _heightmap;
});
}
/* needs fixing if really needed
public float[] ResizeTerrain512NearestNeighbor(float[] heightMap)
{
float[] returnarr = new float[262144];
float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
// Filling out the array into its multi-dimensional components
for (int y = 0; y < WorldExtents.Y; y++)
{
for (int x = 0; x < WorldExtents.X; x++)
{
resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
}
}
// Resize using Nearest Neighbor
// This particular way is quick but it only works on a multiple of the original
// The idea behind this method can be described with the following diagrams
// second pass and third pass happen in the same loop really.. just separated
// them to show what this does.
// First Pass
// ResultArr:
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// Second Pass
// ResultArr2:
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// Third pass fills in the blanks
// ResultArr2:
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// X,Y = .
// X+1,y = ^
// X,Y+1 = *
// X+1,Y+1 = #
// Filling in like this;
// .*
// ^#
// 1st .
// 2nd *
// 3rd ^
// 4th #
// on single loop.
float[,] resultarr2 = new float[512, 512];
for (int y = 0; y < WorldExtents.Y; y++)
{
for (int x = 0; x < WorldExtents.X; x++)
{
resultarr2[y * 2, x * 2] = resultarr[y, x];
if (y < WorldExtents.Y)
{
resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
}
if (x < WorldExtents.X)
{
resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
}
if (x < WorldExtents.X && y < WorldExtents.Y)
{
resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
}
}
}
//Flatten out the array
int i = 0;
for (int y = 0; y < 512; y++)
{
for (int x = 0; x < 512; x++)
{
if (resultarr2[y, x] <= 0)
returnarr[i] = 0.0000001f;
else
returnarr[i] = resultarr2[y, x];
i++;
}
}
return returnarr;
}
public float[] ResizeTerrain512Interpolation(float[] heightMap)
{
float[] returnarr = new float[262144];
float[,] resultarr = new float[512, 512];
// Filling out the array into its multi-dimensional components
for (int y = 0; y < Constants.RegionSize; y++)
{
for (int x = 0; x < Constants.RegionSize; x++)
{
resultarr[y, x] = heightMap[y * m_region.RegionSizeX + x];
}
}
// Resize using interpolation
// This particular way is quick but it only works on a multiple of the original
// The idea behind this method can be described with the following diagrams
// second pass and third pass happen in the same loop really.. just separated
// them to show what this does.
// First Pass
// ResultArr:
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// 1,1,1,1,1,1
// Second Pass
// ResultArr2:
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// ,,,,,,,,,,
// 1,,1,,1,,1,,1,,1,
// Third pass fills in the blanks
// ResultArr2:
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// 1,1,1,1,1,1,1,1,1,1,1,1
// X,Y = .
// X+1,y = ^
// X,Y+1 = *
// X+1,Y+1 = #
// Filling in like this;
// .*
// ^#
// 1st .
// 2nd *
// 3rd ^
// 4th #
// on single loop.
float[,] resultarr2 = new float[512, 512];
for (int y = 0; y < m_region.RegionSizeY; y++)
{
for (int x = 0; x < m_region.RegionSizeX; x++)
{
resultarr2[y * 2, x * 2] = resultarr[y, x];
if (y < m_region.RegionSizeY)
{
if (y + 1 < m_region.RegionSizeY)
{
if (x + 1 < m_region.RegionSizeX)
{
resultarr2[(y * 2) + 1, x * 2] = ((resultarr[y, x] + resultarr[y + 1, x] +
resultarr[y, x + 1] + resultarr[y + 1, x + 1]) / 4);
}
else
{
resultarr2[(y * 2) + 1, x * 2] = ((resultarr[y, x] + resultarr[y + 1, x]) / 2);
}
}
else
{
resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
}
}
if (x < m_region.RegionSizeX)
{
if (x + 1 < m_region.RegionSizeX)
{
if (y + 1 < m_region.RegionSizeY)
{
resultarr2[y * 2, (x * 2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
resultarr[y, x + 1] + resultarr[y + 1, x + 1]) / 4);
}
else
{
resultarr2[y * 2, (x * 2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1]) / 2);
}
}
else
{
resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
}
}
if (x < m_region.RegionSizeX && y < m_region.RegionSizeY)
{
if ((x + 1 < m_region.RegionSizeX) && (y + 1 < m_region.RegionSizeY))
{
resultarr2[(y * 2) + 1, (x * 2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
resultarr[y, x + 1] + resultarr[y + 1, x + 1]) / 4);
}
else
{
resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
}
}
}
}
//Flatten out the array
int i = 0;
for (int y = 0; y < 512; y++)
{
for (int x = 0; x < 512; x++)
{
if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
{
m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
resultarr2[y, x] = 0;
}
returnarr[i] = resultarr2[y, x];
i++;
}
}
return returnarr;
}
*/
#endregion
public override void SetTerrain (short[] heightMap)
{
short[] _heightmap;
if(!ODETerrainHeightFieldHeights.TryGetValue(RegionTerrain, out _heightmap))
_heightmap = new short[((m_region.RegionSizeX + 3) * (m_region.RegionSizeY + 3))];
int heightmapWidth = m_region.RegionSizeX + 2;
int heightmapHeight = m_region.RegionSizeY + 2;
int heightmapWidthSamples = m_region.RegionSizeX + 3; // + one to complete the 256m + 2 margins each side
int heightmapHeightSamples = m_region.RegionSizeY + 3;
float hfmin = 2000;
float hfmax = -2000;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
//Some notes on this part
//xx and yy are used for the original heightmap, as we are offsetting the new one by 1
// so we subtract one so that we can put the heightmap in correctly
int xx = Util.Clip (x - 1, 0, m_region.RegionSizeX - 1);
int yy = Util.Clip (y - 1, 0, m_region.RegionSizeY - 1);
short val = heightMap[yy * m_region.RegionSizeX + xx];
//ODE is evil... flip x and y
_heightmap[(x * heightmapHeightSamples) + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
lock (OdeLock)
{
if (RegionTerrain != IntPtr.Zero)
{
d.SpaceRemove (space, RegionTerrain);
d.GeomDestroy (RegionTerrain);
}
TerrainHeightFieldHeights.Remove (RegionTerrain);
ODETerrainHeightFieldHeights.Remove (RegionTerrain);
TerrainHeightFieldlimits.Remove (RegionTerrain);
actor_name_map.Remove (RegionTerrain);
geom_name_map.Remove (RegionTerrain);
const float scale = (1f / (float)Constants.TerrainCompression);
const float offset = 0.0f;
float thickness = (float)hfmin;
const int wrap = 0;
IntPtr HeightmapData = d.GeomHeightfieldDataCreate ();
d.GeomHeightfieldDataBuildShort (HeightmapData, _heightmap, 0, heightmapHeight, heightmapWidth,
heightmapHeightSamples, heightmapWidthSamples, scale,
offset, thickness, wrap);
d.GeomHeightfieldDataSetBounds (HeightmapData, hfmin, (float)hfmax + 1.0f);
RegionTerrain = d.CreateHeightfield (space, HeightmapData, 1);
if (RegionTerrain != IntPtr.Zero)
{
d.GeomSetCategoryBits (RegionTerrain, (int)(CollisionCategories.Land));
d.GeomSetCollideBits (RegionTerrain, (int)(CollisionCategories.Space));
}
geom_name_map[RegionTerrain] = "Terrain";
NullObjectPhysicsActor terrainActor = new NullObjectPhysicsActor()
{
PhysicsActorType = (int)ActorTypes.Ground
};
actor_name_map[RegionTerrain] = terrainActor;
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 (RegionTerrain, ref R);
d.GeomSetPosition (RegionTerrain, (m_region.RegionSizeX * 0.5f), (m_region.RegionSizeY * 0.5f), 0);
float[] heighlimits = new float[2];
heighlimits[0] = hfmin;
heighlimits[1] = hfmax;
TerrainHeightFieldHeights.Add (RegionTerrain, heightMap);
ODETerrainHeightFieldHeights.Add (RegionTerrain, _heightmap);
TerrainHeightFieldlimits.Add (RegionTerrain, heighlimits);
}
}
public override void SetTerrain(ITerrainChannel channel, short[] heightMap)
{
m_channel = channel;
float[] _heightmap = ODETerrainHeightFieldHeights;
if (ODETerrainHeightFieldHeights == null)
_heightmap = new float[m_region.RegionSizeX * m_region.RegionSizeY];
for (int x = 0; x < m_region.RegionSizeX; x++)
{
for (int y = 0; y < m_region.RegionSizeY; y++)
{
_heightmap[(x * m_region.RegionSizeX) + y] = heightMap[y * m_region.RegionSizeX + x] / Constants.TerrainCompression;
}
}
float hfmin = _heightmap.Min();
float hfmax = _heightmap.Max();
SimulationChangesQueue.Enqueue(() =>
{
if (RegionTerrain != IntPtr.Zero)
{
d.GeomHeightfieldDataDestroy(RegionTerrain);
d.SpaceRemove(space, RegionTerrain);
//d.GeomDestroy(RegionTerrain);
GC.RemoveMemoryPressure(_heightmap.Length);
}
const float scale = 1f;
const float offset = 0.0f;
float thickness = 0.2f;
const int wrap = 0;
IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
GC.AddMemoryPressure(_heightmap.Length);
//Add the memory pressure properly (note: should we be doing this since we have it in managed memory?)
//Do NOT copy it! Otherwise, it'll copy the terrain into unmanaged memory where we can't release it each time
d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, m_region.RegionSizeX, m_region.RegionSizeY,
m_region.RegionSizeX, m_region.RegionSizeY, scale,
offset, thickness, wrap);
d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1.0f, hfmax + 1.0f);
RegionTerrain = d.CreateHeightfield(space, HeightmapData, 1);
d.GeomSetCategoryBits(RegionTerrain, (int)(CollisionCategories.Land));
d.GeomSetCollideBits(RegionTerrain, (int)(CollisionCategories.Space));
actor_name_map[RegionTerrain] = new NullObjectPhysicsActor();
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(RegionTerrain, ref R);
d.GeomSetPosition(RegionTerrain, (m_region.RegionSizeX * 0.5f), (m_region.RegionSizeY * 0.5f), 0);
TerrainHeightFieldHeights = heightMap;
ODETerrainHeightFieldHeights = _heightmap;
});
//Trimesh terrain
/*var mesh = new OpenSim.Region.Physics.Meshing.Mesh(523452345);
for (int i = 0; i < m_region.RegionSizeX - 1; i++)
{
for (int j = 0; j < m_region.RegionSizeY - 1; j++)
{
mesh.Add(new Triangle(
new Vertex(i, j, m_channel[i, j]),
new Vertex(i + 1, j, m_channel[i + 1, j]),
new Vertex(i + 1, j + 1, m_channel[i + 1, j + 1])));
}
}
System.IntPtr vertices, indices;
int vertexCount, indexCount;
int vertexStride, triStride;
mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
// Note, that vertices are fixed in unmanaged heap
mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
// Also fixed, needs release after usage
if (vertexCount == 0 || indexCount == 0)
return;
mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
var _triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount,
triStride);
d.GeomTriMeshDataPreprocess(_triMeshData);
var RegionTerrain = d.CreateTriMesh(space, _triMeshData, null, null, null);
d.GeomSetCategoryBits(RegionTerrain, (int)(CollisionCategories.Land));
d.GeomSetCollideBits(RegionTerrain, (int)(CollisionCategories.Space));
actor_name_map[RegionTerrain] = new NullObjectPhysicsActor();
TerrainHeightFieldHeights = heightMap;*/
}
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)
{
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, 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));
}
/// <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, prim.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, prm.BadMeshAssetCollideBits);
}
else
{
d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int)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;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
{
prm.createAMotor(m_angularlock);
}
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, BadMeshAssetCollideBits);
}
else
{
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
d.GeomSetCollideBits(prim_geom, (int)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
if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _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);
}
}
/*public void SetTerrain(float[] heightMap, Vector3 pOffset)
{
// this._heightmap[i] = (double)heightMap[i];
// dbm (danx0r) -- creating a buffer zone of one extra sample all around
//_origheightmap = heightMap;
float[] _heightmap;
// zero out a heightmap array float array (single dimension [flattened]))
//if ((int)Constants.RegionSize == 256)
// _heightmap = new float[514 * 514];
//else
_heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
uint heightmapWidth = Constants.RegionSize + 1;
uint heightmapHeight = Constants.RegionSize + 1;
uint heightmapWidthSamples;
uint heightmapHeightSamples;
//if (((int)Constants.RegionSize) == 256)
//{
// heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
// heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
// heightmapWidth++;
// heightmapHeight++;
//}
//else
//{
heightmapWidthSamples = (uint)Constants.RegionSize + 1;
heightmapHeightSamples = (uint)Constants.RegionSize + 1;
//}
const float scale = 1.0f;
const float offset = 0.0f;
const float thickness = 1.0f;
const int wrap = 0;
int regionsize = (int) Constants.RegionSize + 2;
//Double resolution
//if (((int)Constants.RegionSize) == 256)
// heightMap = ResizeTerrain512Interpolation(heightMap);
// if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256)
// regionsize = 512;
float hfmin = 2000;
float hfmax = -2000;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
int xx = Util.Clip(x - 1, 0, regionsize - 1);
int yy = Util.Clip(y - 1, 0, regionsize - 1);
float val= heightMap[yy * (int)Constants.RegionSize + xx];
_heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
lock (OdeLock)
{
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 + 1, heightmapHeight + 1,
(int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, 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);
//Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
q1 = q1 * q2;
//q1 = q1 * q3;
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 + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0);
IntPtr testGround = IntPtr.Zero;
if (RegionTerrain.TryGetValue(pOffset, out testGround))
{
RegionTerrain.Remove(pOffset);
}
RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
}
}*/
public void SetTerrain(float[] heightMap, double[,] normalHeightMap, Vector3 pOffset)
{
float[] _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
uint heightmapWidth = Constants.RegionSize + 1;
uint heightmapHeight = Constants.RegionSize + 1;
uint heightmapWidthSamples = (uint)Constants.RegionSize + 2;
uint heightmapHeightSamples = (uint)Constants.RegionSize + 2;
if (Constants.RegionSize == 256)
{
// -- creating a buffer zone of one extra sample all around - danzor
heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
heightmapWidth++;
heightmapHeight++;
}
int regionsize = (int)Constants.RegionSize;
float hfmin = 2000;
float hfmax = -2000;
if (regionsize == 256)
{
//Double resolution
_heightmap = new float[((((int)Constants.RegionSize * 2) + 2) * (((int)Constants.RegionSize * 2) + 2))];
heightMap = ResizeTerrain512Interpolation(heightMap);
regionsize *= 2;
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
int xx = Util.Clip(x - 1, 0, (regionsize - 1) - 1);
int yy = Util.Clip(y - 1, 0, (regionsize - 1) - 1);
float val = heightMap[yy * regionsize + xx];
_heightmap[x * heightmapWidthSamples + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
}
else
{
for (int x = 0; x < heightmapWidthSamples; x++)
{
for (int y = 0; y < heightmapHeightSamples; y++)
{
int xx = Util.Clip(x - 1, 0, regionsize - 1);
int yy = Util.Clip(y - 1, 0, regionsize - 1);
float val = heightMap[yy * (int)Constants.RegionSize + xx];
_heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
hfmin = (val < hfmin) ? val : hfmin;
hfmax = (val > hfmax) ? val : hfmax;
}
}
}
lock (OdeLock)
{
IntPtr GroundGeom = IntPtr.Zero;
if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
{
if (GroundGeom != IntPtr.Zero)
{
d.SpaceRemove(space, GroundGeom);
d.GeomDestroy(GroundGeom);
}
RegionTerrain.Remove(pOffset);
TerrainHeightFieldHeights.Remove(GroundGeom);
NormalTerrainHeightFieldHeights.Remove(GroundGeom);
actor_name_map.Remove(GroundGeom);
geom_name_map.Remove(GroundGeom);
}
const float scale = 1.0f;
const float offset = 0.0f;
const float thickness = 0.1f;
const int wrap = 0;
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";
NullPhysicsActor terrainActor = new NullPhysicsActor()
{
PhysicsActorType = (int)ActorTypes.Ground
};
actor_name_map[GroundGeom] = terrainActor;
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 + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0);
RegionTerrain.Remove(pOffset);
RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
NormalTerrainHeightFieldHeights.Add(GroundGeom, normalHeightMap);
}
}
public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
