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);