123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823 |
- /*
- * Copyright (c) Contributors, http://opensimulator.org/
- * See CONTRIBUTORS.TXT for a full list of copyright holders.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the OpenSimulator Project nor the
- * names of its contributors may be used to endorse or promote products
- * derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
- // Revision 2011/12/13 by Ubit Umarov
- //#define SPAM
- using System;
- using System.Collections.Generic;
- using System.Linq;
- using System.Reflection;
- using System.Runtime.InteropServices;
- using System.Threading;
- using System.IO;
- using System.Diagnostics;
- using log4net;
- using Nini.Config;
- using Mono.Addins;
- using OdeAPI;
- using OpenSim.Framework;
- using OpenSim.Region.Framework.Scenes;
- using OpenSim.Region.Framework.Interfaces;
- using OpenSim.Region.PhysicsModules.SharedBase;
- using OpenMetaverse;
- namespace OpenSim.Region.PhysicsModule.ubOde
- {
- // colision flags of things others can colide with
- // rays, sensors, probes removed since can't be colided with
- // The top space where things are placed provided further selection
- // ie physical are in active space nonphysical in static
- // this should be exclusive as possible
- [Flags]
- public enum CollisionCategories : uint
- {
- Disabled = 0,
- //by 'things' types
- Space = 0x01,
- Geom = 0x02, // aka prim/part
- Character = 0x04,
- Land = 0x08,
- Water = 0x010,
- // by state
- Phantom = 0x01000,
- VolumeDtc = 0x02000,
- Selected = 0x04000,
- NoShape = 0x08000,
- All = 0xffffffff
- }
- /// <summary>
- /// Material type for a primitive
- /// </summary>
- public enum Material : int
- {
- /// <summary></summary>
- Stone = 0,
- /// <summary></summary>
- Metal = 1,
- /// <summary></summary>
- Glass = 2,
- /// <summary></summary>
- Wood = 3,
- /// <summary></summary>
- Flesh = 4,
- /// <summary></summary>
- Plastic = 5,
- /// <summary></summary>
- Rubber = 6,
- light = 7 // compatibility with old viewers
- }
- public enum changes : int
- {
- Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
- Remove,
- Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
- // or removes from a object if arg is null
- DeLink,
- Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
- Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
- PosOffset, // not in use
- // arg Vector3 new position in local coords. Changes prim position in object
- OriOffset, // not in use
- // arg Vector3 new position in local coords. Changes prim position in object
- Velocity,
- TargetVelocity,
- AngVelocity,
- Acceleration,
- Force,
- Torque,
- Momentum,
- AddForce,
- AddAngForce,
- AngLock,
- Buoyancy,
- PIDTarget,
- PIDTau,
- PIDActive,
- PIDHoverHeight,
- PIDHoverType,
- PIDHoverTau,
- PIDHoverActive,
- Size,
- AvatarSize,
- Shape,
- PhysRepData,
- AddPhysRep,
- CollidesWater,
- VolumeDtc,
- Physical,
- Phantom,
- Selected,
- disabled,
- building,
- VehicleType,
- VehicleFloatParam,
- VehicleVectorParam,
- VehicleRotationParam,
- VehicleFlags,
- SetVehicle,
- SetInertia,
- Null //keep this last used do dim the methods array. does nothing but pulsing the prim
- }
- public struct ODEchangeitem
- {
- public PhysicsActor actor;
- public OdeCharacter character;
- public changes what;
- public Object arg;
- }
- public class ODEScene : PhysicsScene
- {
- private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
- public bool m_OSOdeLib = false;
- public Scene m_frameWorkScene = null;
- // private int threadid = 0;
- // const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
- const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2;
- const float comumContactERP = 0.75f;
- const float comumContactCFM = 0.0001f;
- const float comumContactSLIP = 0f;
- float frictionMovementMult = 0.8f;
- float TerrainBounce = 0.001f;
- float TerrainFriction = 0.3f;
- public float AvatarFriction = 0;// 0.9f * 0.5f;
- // this netx dimensions are only relevant for terrain partition (mega regions)
- // WorldExtents below has the simulation dimensions
- // they should be identical except on mega regions
- private uint m_regionWidth = Constants.RegionSize;
- private uint m_regionHeight = Constants.RegionSize;
- public float ODE_STEPSIZE = 0.020f;
- public float HalfOdeStep = 0.01f;
- public int odetimestepMS = 20; // rounded
- private float metersInSpace = 25.6f;
- private float m_timeDilation = 1.0f;
- private DateTime m_lastframe;
- private DateTime m_lastMeshExpire;
- public float gravityx = 0f;
- public float gravityy = 0f;
- public float gravityz = -9.8f;
- private float waterlevel = 0f;
- private int framecount = 0;
- private float avDensity = 80f;
- private float avMovementDivisorWalk = 1.3f;
- private float avMovementDivisorRun = 0.8f;
- private float minimumGroundFlightOffset = 3f;
- public float maximumMassObject = 10000.01f;
- public float geomDefaultDensity = 10.0f;
- public float maximumAngularVelocity = 12.0f; // default 12rad/s
- public float maxAngVelocitySQ = 144f; // squared value
- public float bodyPIDD = 35f;
- public float bodyPIDG = 25;
- public int bodyFramesAutoDisable = 5;
- private d.NearCallback nearCallback;
- private Dictionary<uint,OdePrim> _prims = new Dictionary<uint,OdePrim>();
- private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
- private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
- private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
- public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
- /// <summary>
- /// A list of actors that should receive collision events.
- /// </summary>
- private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
- private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
- private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
- public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
- private float contactsurfacelayer = 0.002f;
- private int contactsPerCollision = 80;
- internal IntPtr ContactgeomsArray = IntPtr.Zero;
- private IntPtr GlobalContactsArray = IntPtr.Zero;
- private d.Contact SharedTmpcontact = new d.Contact();
- const int maxContactsbeforedeath = 6000;
- private volatile int m_global_contactcount = 0;
- private IntPtr contactgroup;
- public ContactData[] m_materialContactsData = new ContactData[8];
- private IntPtr TerrainGeom;
- private float[] TerrainHeightFieldHeight;
- private GCHandle TerrainHeightFieldHeightsHandler = new GCHandle();
- private int m_physicsiterations = 15;
- private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
- // private PhysicsActor PANull = new NullPhysicsActor();
- private float step_time = 0.0f;
- public IntPtr world;
- // split the spaces acording to contents type
- // ActiveSpace contains characters and active prims
- // StaticSpace contains land and other that is mostly static in enviroment
- // this can contain subspaces, like the grid in staticspace
- // as now space only contains this 2 top spaces
- public IntPtr TopSpace; // the global space
- public IntPtr ActiveSpace; // space for active prims
- public IntPtr CharsSpace; // space for active prims
- public IntPtr StaticSpace; // space for the static things around
- public IntPtr GroundSpace; // space for ground
- // some speedup variables
- private int spaceGridMaxX;
- private int spaceGridMaxY;
- private float spacesPerMeterX;
- private float spacesPerMeterY;
- // split static geometry collision into a grid as before
- private IntPtr[,] staticPrimspace;
- private IntPtr[] staticPrimspaceOffRegion;
- public Object OdeLock;
- public static Object SimulationLock;
- public IMesher mesher;
- public IConfigSource m_config;
- public bool physics_logging = false;
- public int physics_logging_interval = 0;
- public bool physics_logging_append_existing_logfile = false;
- public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
- private ODERayCastRequestManager m_rayCastManager;
- public ODEMeshWorker m_meshWorker;
- /* maybe needed if ode uses tls
- private void checkThread()
- {
- int th = Thread.CurrentThread.ManagedThreadId;
- if(th != threadid)
- {
- threadid = th;
- d.AllocateODEDataForThread(~0U);
- }
- }
- */
- IConfig physicsconfig = null;
- public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib)
- {
- OdeLock = new Object();
- EngineType = pname;
- PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
- EngineName = pname + " " + pversion;
- m_config = psourceconfig;
- m_OSOdeLib = pOSOdeLib;
- // m_OSOdeLib = false; //debug
- m_frameWorkScene = pscene;
- m_frameWorkScene.RegisterModuleInterface<PhysicsScene>(this);
- Initialization();
- base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
- (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
- (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
- }
- public void RegionLoaded()
- {
- mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
- if (mesher == null)
- {
- m_log.ErrorFormat("[ubOde] No mesher. module disabled");
- return;
- }
- m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
- m_frameWorkScene.PhysicsEnabled = true;
- }
- /// <summary>
- /// Initiailizes the scene
- /// Sets many properties that ODE requires to be stable
- /// These settings need to be tweaked 'exactly' right or weird stuff happens.
- /// </summary>
- private void Initialization()
- {
- d.AllocateODEDataForThread(~0U);
- SimulationLock = new Object();
- nearCallback = near;
- m_rayCastManager = new ODERayCastRequestManager(this);
- WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX;
- m_regionWidth = (uint)WorldExtents.X;
- WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY;
- m_regionHeight = (uint)WorldExtents.Y;
- lock (OdeLock)
- {
- // Create the world and the first space
- try
- {
- world = d.WorldCreate();
- TopSpace = d.HashSpaceCreate(IntPtr.Zero);
- // now the major subspaces
- ActiveSpace = d.HashSpaceCreate(TopSpace);
- CharsSpace = d.HashSpaceCreate(TopSpace);
- StaticSpace = d.HashSpaceCreate(TopSpace);
- GroundSpace = d.HashSpaceCreate(TopSpace);
- }
- catch
- {
- // i must RtC#FM
- // i did!
- }
- d.HashSpaceSetLevels(TopSpace, -5, 12);
- d.HashSpaceSetLevels(ActiveSpace, -5, 10);
- d.HashSpaceSetLevels(CharsSpace, -4, 3);
- d.HashSpaceSetLevels(StaticSpace, -5, 12);
- d.HashSpaceSetLevels(GroundSpace, 0, 8);
- // demote to second level
- d.SpaceSetSublevel(ActiveSpace, 1);
- d.SpaceSetSublevel(CharsSpace, 1);
- d.SpaceSetSublevel(StaticSpace, 1);
- d.SpaceSetSublevel(GroundSpace, 1);
- d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
- CollisionCategories.Geom |
- CollisionCategories.Character |
- CollisionCategories.Phantom |
- CollisionCategories.VolumeDtc
- ));
- d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
- CollisionCategories.Geom |
- CollisionCategories.Character |
- CollisionCategories.Phantom |
- CollisionCategories.VolumeDtc
- ));
- d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
- CollisionCategories.Geom |
- CollisionCategories.Character |
- CollisionCategories.Phantom |
- CollisionCategories.VolumeDtc
- ));
- d.GeomSetCollideBits(CharsSpace, 0);
- d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
- CollisionCategories.Geom |
- // CollisionCategories.Land |
- // CollisionCategories.Water |
- CollisionCategories.Phantom |
- CollisionCategories.VolumeDtc
- ));
- d.GeomSetCollideBits(StaticSpace, 0);
- d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
- d.GeomSetCollideBits(GroundSpace, 0);
- contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1);
- //contactgroup
- d.WorldSetAutoDisableFlag(world, false);
- }
- // checkThread();
- // Defaults
- int contactsPerCollision = 80;
- physicsconfig = null;
- if (m_config != null)
- {
- physicsconfig = m_config.Configs["ODEPhysicsSettings"];
- if (physicsconfig != null)
- {
- gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
- gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
- gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
- metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
- // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
- ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
- avDensity = physicsconfig.GetFloat("av_density", avDensity);
- avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
- avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
- contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
- geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
- bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
- physics_logging = physicsconfig.GetBoolean("physics_logging", false);
- physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
- physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
- minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
- maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
- avDensity *= 3f / 80f; // scale other engines density option to this
- }
- }
- float heartbeat = 1/m_frameWorkScene.FrameTime;
- maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
- maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
- d.WorldSetCFM(world, comumContactCFM);
- d.WorldSetERP(world, comumContactERP);
- d.WorldSetGravity(world, gravityx, gravityy, gravityz);
- d.WorldSetLinearDamping(world, 0.001f);
- d.WorldSetAngularDamping(world, 0.002f);
- d.WorldSetAngularDampingThreshold(world, 0f);
- d.WorldSetLinearDampingThreshold(world, 0f);
- d.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);
- d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
- d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
- d.WorldSetContactMaxCorrectingVel(world, 60.0f);
- HalfOdeStep = ODE_STEPSIZE * 0.5f;
- odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);
- ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
- GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * d.Contact.unmanagedSizeOf);
- SharedTmpcontact.geom.g1 = IntPtr.Zero;
- SharedTmpcontact.geom.g2 = IntPtr.Zero;
- SharedTmpcontact.geom.side1 = -1;
- SharedTmpcontact.geom.side2 = -1;
- SharedTmpcontact.surface.mode = comumContactFlags;
- SharedTmpcontact.surface.mu = 0;
- SharedTmpcontact.surface.bounce = 0;
- SharedTmpcontact.surface.bounce_vel = 1.5f;
- SharedTmpcontact.surface.soft_cfm = comumContactCFM;
- SharedTmpcontact.surface.soft_erp = comumContactERP;
- SharedTmpcontact.surface.slip1 = comumContactSLIP;
- SharedTmpcontact.surface.slip2 = comumContactSLIP;
- m_materialContactsData[(int)Material.Stone].mu = 0.8f;
- m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
- m_materialContactsData[(int)Material.Metal].mu = 0.3f;
- m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
- m_materialContactsData[(int)Material.Glass].mu = 0.2f;
- m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
- m_materialContactsData[(int)Material.Wood].mu = 0.6f;
- m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
- m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
- m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
- m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
- m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
- m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
- m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
- m_materialContactsData[(int)Material.light].mu = 0.0f;
- m_materialContactsData[(int)Material.light].bounce = 0.0f;
- spacesPerMeterX = 1.0f / metersInSpace;
- spacesPerMeterY = spacesPerMeterX;
- spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
- spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
- if (spaceGridMaxX > 24)
- {
- spaceGridMaxX = 24;
- spacesPerMeterX = spaceGridMaxX / WorldExtents.X;
- }
- if (spaceGridMaxY > 24)
- {
- spaceGridMaxY = 24;
- spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
- }
- staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
- // create all spaces now
- int i, j;
- IntPtr newspace;
- for (i = 0; i < spaceGridMaxX; i++)
- for (j = 0; j < spaceGridMaxY; j++)
- {
- newspace = d.HashSpaceCreate(StaticSpace);
- d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
- waitForSpaceUnlock(newspace);
- d.SpaceSetSublevel(newspace, 2);
- d.HashSpaceSetLevels(newspace, -2, 8);
- d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
- CollisionCategories.Geom |
- CollisionCategories.Land |
- CollisionCategories.Water |
- CollisionCategories.Phantom |
- CollisionCategories.VolumeDtc
- ));
- d.GeomSetCollideBits(newspace, 0);
- staticPrimspace[i, j] = newspace;
- }
- // let this now be index limit
- spaceGridMaxX--;
- spaceGridMaxY--;
- // create 4 off world spaces (x<0,x>max,y<0,y>max)
- staticPrimspaceOffRegion = new IntPtr[4];
- for (i = 0; i < 4; i++)
- {
- newspace = d.HashSpaceCreate(StaticSpace);
- d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
- waitForSpaceUnlock(newspace);
- d.SpaceSetSublevel(newspace, 2);
- d.HashSpaceSetLevels(newspace, -2, 8);
- d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
- CollisionCategories.Geom |
- CollisionCategories.Land |
- CollisionCategories.Water |
- CollisionCategories.Phantom |
- CollisionCategories.VolumeDtc
- ));
- d.GeomSetCollideBits(newspace, 0);
- staticPrimspaceOffRegion[i] = newspace;
- }
- m_lastframe = DateTime.UtcNow;
- m_lastMeshExpire = m_lastframe;
- }
- internal void waitForSpaceUnlock(IntPtr space)
- {
- //if (space != IntPtr.Zero)
- //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
- }
- #region Collision Detection
- // sets a global contact for a joint for contactgeom , and base contact description)
- private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom,bool smooth)
- {
- if (m_global_contactcount >= maxContactsbeforedeath)
- return IntPtr.Zero;
- m_global_contactcount++;
- if(smooth)
- SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f;
- else
- SharedTmpcontact.geom.depth = contactGeom.depth;
- SharedTmpcontact.geom.pos = contactGeom.pos;
- SharedTmpcontact.geom.normal = contactGeom.normal;
- IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
- Marshal.StructureToPtr(SharedTmpcontact, contact, true);
- return d.JointCreateContactPtr(world, contactgroup, contact);
- }
- private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
- {
- if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
- return false;
- IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
- newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
- return true;
- }
- /// <summary>
- /// This is our near callback. A geometry is near a body
- /// </summary>
- /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
- /// <param name="g1">a geometry or space</param>
- /// <param name="g2">another geometry or space</param>
- ///
- private void near(IntPtr space, IntPtr g1, IntPtr g2)
- {
- // no lock here! It's invoked from within Simulate(), which is thread-locked
- if (m_global_contactcount >= maxContactsbeforedeath)
- return;
- // Test if we're colliding a geom with a space.
- // If so we have to drill down into the space recursively
- if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
- return;
- if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
- {
- // We'll be calling near recursivly if one
- // of them is a space to find all of the
- // contact points in the space
- try
- {
- d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
- }
- catch (AccessViolationException)
- {
- m_log.Warn("[PHYSICS]: Unable to collide test a space");
- return;
- }
- //here one should check collisions of geoms inside a space
- // but on each space we only should have geoms that not colide amoung each other
- // so we don't dig inside spaces
- return;
- }
- // get geom bodies to check if we already a joint contact
- // guess this shouldn't happen now
- IntPtr b1 = d.GeomGetBody(g1);
- IntPtr b2 = d.GeomGetBody(g2);
- // d.GeomClassID id = d.GeomGetClass(g1);
- // Figure out how many contact points we have
- int count = 0;
- try
- {
- // Colliding Geom To Geom
- // This portion of the function 'was' blatantly ripped off from BoxStack.cs
- if (g1 == g2)
- return; // Can't collide with yourself
- // if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
- // return;
- /*
- // debug
- PhysicsActor dp2;
- if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
- {
- d.AABB aabb;
- d.GeomGetAABB(g2, out aabb);
- float x = aabb.MaxX - aabb.MinX;
- float y = aabb.MaxY - aabb.MinY;
- float z = aabb.MaxZ - aabb.MinZ;
- if (x > 60.0f || y > 60.0f || z > 60.0f)
- {
- if (!actor_name_map.TryGetValue(g2, out dp2))
- m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
- else
- m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
- dp2.Name, dp2.Size, x, y, z,
- dp2.Position.ToString(),
- dp2.Orientation.ToString(),
- dp2.Orientation.Length());
- return;
- }
- }
- //
- */
- if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
- d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
- {
- int cflags;
- unchecked
- {
- cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
- }
- count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
- }
- else
- count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
- }
- catch (SEHException)
- {
- m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
- // ode.drelease(world);
- base.TriggerPhysicsBasedRestart();
- }
- catch (Exception e)
- {
- m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
- return;
- }
- // contacts done
- if (count == 0)
- return;
- // try get physical actors
- PhysicsActor p1;
- PhysicsActor p2;
- if (!actor_name_map.TryGetValue(g1, out p1))
- {
- m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
- return;
- }
- if (!actor_name_map.TryGetValue(g2, out p2))
- {
- m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
- return;
- }
- // get first contact
- d.ContactGeom curContact = new d.ContactGeom();
- if (!GetCurContactGeom(0, ref curContact))
- return;
- ContactPoint maxDepthContact = new ContactPoint();
- // do volume detection case
- if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
- {
- maxDepthContact = new ContactPoint(
- new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
- new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
- curContact.depth, false
- );
- collision_accounting_events(p1, p2, maxDepthContact);
- return;
- }
- // big messy collision analises
- float mu = 0;
- float bounce = 0;
- // bool IgnoreNegSides = false;
- ContactData contactdata1 = new ContactData(0, 0, false);
- ContactData contactdata2 = new ContactData(0, 0, false);
- bool dop1ava = false;
- bool dop2ava = false;
- bool ignore = false;
- bool smoothMesh = false;
- switch (p1.PhysicsActorType)
- {
- case (int)ActorTypes.Agent:
- {
- dop1ava = true;
- switch (p2.PhysicsActorType)
- {
- case (int)ActorTypes.Agent:
- case (int)ActorTypes.Prim:
- break;
- default:
- ignore = true; // avatar to terrain and water ignored
- break;
- }
- break;
- }
- case (int)ActorTypes.Prim:
- {
- switch (p2.PhysicsActorType)
- {
- case (int)ActorTypes.Agent:
- dop2ava = true;
- break;
- case (int)ActorTypes.Prim:
- Vector3 relV = p1.rootVelocity - p2.rootVelocity;
- float relVlenSQ = relV.LengthSquared();
- if (relVlenSQ > 0.0001f)
- {
- p1.CollidingObj = true;
- p2.CollidingObj = true;
- }
- p1.getContactData(ref contactdata1);
- p2.getContactData(ref contactdata2);
- bounce = contactdata1.bounce * contactdata2.bounce;
- mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
- if (relVlenSQ > 0.01f)
- mu *= frictionMovementMult;
- if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass &&
- d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
- smoothMesh = true;
- break;
- case (int)ActorTypes.Ground:
- p1.getContactData(ref contactdata1);
- bounce = contactdata1.bounce * TerrainBounce;
- mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
- Vector3 v1 = p1.rootVelocity;
- if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
- mu *= frictionMovementMult;
- p1.CollidingGround = true;
- if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
- smoothMesh = true;
- break;
- case (int)ActorTypes.Water:
- default:
- ignore = true;
- break;
- }
- }
- break;
- case (int)ActorTypes.Ground:
- if (p2.PhysicsActorType == (int)ActorTypes.Prim)
- {
- p2.CollidingGround = true;
- p2.getContactData(ref contactdata2);
- bounce = contactdata2.bounce * TerrainBounce;
- mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
- // if (curContact.side1 > 0) // should be 2 ?
- // IgnoreNegSides = true;
- Vector3 v2 = p2.rootVelocity;
- if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
- mu *= frictionMovementMult;
- if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass)
- smoothMesh = true;
- }
- else
- ignore = true;
- break;
- case (int)ActorTypes.Water:
- default:
- break;
- }
- if (ignore)
- return;
- IntPtr Joint;
- bool FeetCollision = false;
- int ncontacts = 0;
- int i = 0;
- maxDepthContact = new ContactPoint();
- maxDepthContact.PenetrationDepth = float.MinValue;
- ContactPoint minDepthContact = new ContactPoint();
- minDepthContact.PenetrationDepth = float.MaxValue;
- SharedTmpcontact.geom.depth = 0;
- SharedTmpcontact.surface.mu = mu;
- SharedTmpcontact.surface.bounce = bounce;
- d.ContactGeom altContact = new d.ContactGeom();
- bool useAltcontact = false;
- bool noskip = true;
- if(dop1ava || dop2ava)
- smoothMesh = false;
- while (true)
- {
- noskip = true;
- useAltcontact = false;
- if (dop1ava)
- {
- if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
- {
- if (p2.PhysicsActorType == (int)ActorTypes.Agent)
- {
- p1.CollidingObj = true;
- p2.CollidingObj = true;
- }
- else if (p2.rootVelocity.LengthSquared() > 0.0f)
- p2.CollidingObj = true;
- }
- else
- noskip = false;
- }
- else if (dop2ava)
- {
- if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
- {
- if (p1.PhysicsActorType == (int)ActorTypes.Agent)
- {
- p1.CollidingObj = true;
- p2.CollidingObj = true;
- }
- else if (p1.rootVelocity.LengthSquared() > 0.0f)
- p1.CollidingObj = true;
- }
- else
- noskip = false;
- }
- if (noskip)
- {
- if(useAltcontact)
- Joint = CreateContacJoint(ref altContact,smoothMesh);
- else
- Joint = CreateContacJoint(ref curContact,smoothMesh);
- if (Joint == IntPtr.Zero)
- break;
- d.JointAttach(Joint, b1, b2);
- ncontacts++;
- if (curContact.depth > maxDepthContact.PenetrationDepth)
- {
- maxDepthContact.Position.X = curContact.pos.X;
- maxDepthContact.Position.Y = curContact.pos.Y;
- maxDepthContact.Position.Z = curContact.pos.Z;
- maxDepthContact.PenetrationDepth = curContact.depth;
- maxDepthContact.CharacterFeet = FeetCollision;
- }
- if (curContact.depth < minDepthContact.PenetrationDepth)
- {
- minDepthContact.PenetrationDepth = curContact.depth;
- minDepthContact.SurfaceNormal.X = curContact.normal.X;
- minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
- minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
- }
- }
- if (++i >= count)
- break;
- if (!GetCurContactGeom(i, ref curContact))
- break;
- }
- if (ncontacts > 0)
- {
- maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
- maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
- maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
- collision_accounting_events(p1, p2, maxDepthContact);
- }
- }
- private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
- {
- uint obj2LocalID = 0;
- // update actors collision score
- if (p1.CollisionScore < float.MaxValue)
- p1.CollisionScore += 1.0f;
- if (p2.CollisionScore < float.MaxValue)
- p2.CollisionScore += 1.0f;
- bool p1events = p1.SubscribedEvents();
- bool p2events = p2.SubscribedEvents();
- if (p1.IsVolumeDtc)
- p2events = false;
- if (p2.IsVolumeDtc)
- p1events = false;
- if (!p2events && !p1events)
- return;
- Vector3 vel = Vector3.Zero;
- if (p2 != null && p2.IsPhysical)
- vel = p2.rootVelocity;
- if (p1 != null && p1.IsPhysical)
- vel -= p1.rootVelocity;
- contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
- switch ((ActorTypes)p1.PhysicsActorType)
- {
- case ActorTypes.Agent:
- case ActorTypes.Prim:
- {
- switch ((ActorTypes)p2.PhysicsActorType)
- {
- case ActorTypes.Agent:
- case ActorTypes.Prim:
- if (p2events)
- {
- //AddCollisionEventReporting(p2);
- p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
- }
- else if(p1.IsVolumeDtc)
- p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact);
- obj2LocalID = p2.ParentActor.LocalID;
- break;
- case ActorTypes.Ground:
- case ActorTypes.Unknown:
- default:
- obj2LocalID = 0;
- break;
- }
- if (p1events)
- {
- contact.SurfaceNormal = -contact.SurfaceNormal;
- contact.RelativeSpeed = -contact.RelativeSpeed;
- //AddCollisionEventReporting(p1);
- p1.AddCollisionEvent(obj2LocalID, contact);
- }
- else if(p2.IsVolumeDtc)
- {
- contact.SurfaceNormal = -contact.SurfaceNormal;
- contact.RelativeSpeed = -contact.RelativeSpeed;
- //AddCollisionEventReporting(p1);
- p1.AddVDTCCollisionEvent(obj2LocalID, contact);
- }
- break;
- }
- case ActorTypes.Ground:
- case ActorTypes.Unknown:
- default:
- {
- if (p2events && !p2.IsVolumeDtc)
- {
- //AddCollisionEventReporting(p2);
- p2.AddCollisionEvent(0, contact);
- }
- break;
- }
- }
- }
- /// <summary>
- /// This is our collision testing routine in ODE
- /// </summary>
- /// <param name="timeStep"></param>
- private void collision_optimized()
- {
- lock (_characters)
- {
- try
- {
- foreach (OdeCharacter chr in _characters)
- {
- if (chr == null)
- continue;
- chr.IsColliding = false;
- // chr.CollidingGround = false; not done here
- chr.CollidingObj = false;
- if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
- continue;
- // do colisions with static space
- d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
- // no coll with gnd
- }
- // chars with chars
- d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
- }
- catch (AccessViolationException)
- {
- m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
- }
- }
- lock (_activeprims)
- {
- foreach (OdePrim aprim in _activeprims)
- {
- aprim.CollisionScore = 0;
- aprim.IsColliding = false;
- if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body))
- aprim.clearSleeperCollisions();
- }
- }
- lock (_activegroups)
- {
- try
- {
- foreach (OdePrim aprim in _activegroups)
- {
- if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body) &&
- aprim.collide_geom != IntPtr.Zero)
- {
- d.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
- d.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
- }
- }
- }
- catch (Exception e)
- {
- m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message);
- }
- }
- // colide active amoung them
- try
- {
- d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
- }
- catch (Exception e)
- {
- m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message);
- }
- // and with chars
- try
- {
- d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
- }
- catch (Exception e)
- {
- m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message);
- }
- }
- #endregion
- /// <summary>
- /// Add actor to the list that should receive collision events in the simulate loop.
- /// </summary>
- /// <param name="obj"></param>
- public void AddCollisionEventReporting(PhysicsActor obj)
- {
- if (!_collisionEventPrim.Contains(obj))
- _collisionEventPrim.Add(obj);
- }
- /// <summary>
- /// Remove actor from the list that should receive collision events in the simulate loop.
- /// </summary>
- /// <param name="obj"></param>
- public void RemoveCollisionEventReporting(PhysicsActor obj)
- {
- lock(_collisionEventPrimRemove)
- {
- if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
- _collisionEventPrimRemove.Add(obj);
- }
- }
- public override float TimeDilation
- {
- get { return m_timeDilation; }
- }
- public override bool SupportsNINJAJoints
- {
- get { return false; }
- }
- #region Add/Remove Entities
- public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
- {
- return null;
- }
- public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
- {
- OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
- size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
- newAv.Flying = isFlying;
- newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
- return newAv;
- }
- public void AddCharacter(OdeCharacter chr)
- {
- lock (_characters)
- {
- if (!_characters.Contains(chr))
- {
- _characters.Add(chr);
- if (chr.bad)
- m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
- }
- }
- }
- public void RemoveCharacter(OdeCharacter chr)
- {
- lock (_characters)
- {
- if (_characters.Contains(chr))
- {
- _characters.Remove(chr);
- }
- }
- }
- public void BadCharacter(OdeCharacter chr)
- {
- lock (_badCharacter)
- {
- if (!_badCharacter.Contains(chr))
- _badCharacter.Add(chr);
- }
- }
- public override void RemoveAvatar(PhysicsActor actor)
- {
- //m_log.Debug("[PHYSICS]:ODELOCK");
- lock (OdeLock)
- {
- d.AllocateODEDataForThread(0);
- ((OdeCharacter) actor).Destroy();
- }
- }
- public void addActivePrim(OdePrim activatePrim)
- {
- // adds active prim..
- lock (_activeprims)
- {
- if (!_activeprims.Contains(activatePrim))
- _activeprims.Add(activatePrim);
- }
- }
- public void addActiveGroups(OdePrim activatePrim)
- {
- lock (_activegroups)
- {
- if (!_activegroups.Contains(activatePrim))
- _activegroups.Add(activatePrim);
- }
- }
- private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
- PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
- {
- OdePrim newPrim;
- lock (OdeLock)
- {
- newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
- }
- return newPrim;
- }
- public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
- Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
- {
- return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
- }
- public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
- Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
- {
- return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
- }
- public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
- Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
- {
- return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
- }
- public void remActivePrim(OdePrim deactivatePrim)
- {
- lock (_activeprims)
- {
- _activeprims.Remove(deactivatePrim);
- }
- }
- public void remActiveGroup(OdePrim deactivatePrim)
- {
- lock (_activegroups)
- {
- _activegroups.Remove(deactivatePrim);
- }
- }
- public override void RemovePrim(PhysicsActor prim)
- {
- // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
- // removed in the next physics simulate pass.
- if (prim is OdePrim)
- {
- // lock (OdeLock)
- {
- OdePrim p = (OdePrim)prim;
- p.setPrimForRemoval();
- }
- }
- }
- public void RemovePrimThreadLocked(OdePrim prim)
- {
- //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
- lock (prim)
- {
- // RemoveCollisionEventReporting(prim);
- lock (_prims)
- _prims.Remove(prim.LocalID);
- }
- }
- public void addToPrims(OdePrim prim)
- {
- lock (_prims)
- _prims[prim.LocalID] = prim;
- }
- public OdePrim getPrim(uint id)
- {
- lock (_prims)
- {
- if(_prims.ContainsKey(id))
- return _prims[id];
- else
- return null;
- }
- }
- public bool havePrim(OdePrim prm)
- {
- lock (_prims)
- return _prims.ContainsKey(prm.LocalID);
- }
- public void changePrimID(OdePrim prim,uint oldID)
- {
- lock (_prims)
- {
- if(_prims.ContainsKey(oldID))
- _prims.Remove(oldID);
- _prims[prim.LocalID] = prim;
- }
- }
- public bool haveActor(PhysicsActor actor)
- {
- if (actor is OdePrim)
- {
- lock (_prims)
- return _prims.ContainsKey(((OdePrim)actor).LocalID);
- }
- else if (actor is OdeCharacter)
- {
- lock (_characters)
- return _characters.Contains((OdeCharacter)actor);
- }
- return false;
- }
- #endregion
- #region Space Separation Calculation
- /// <summary>
- /// Called when a static prim moves or becomes static
- /// Places the prim in a space one the static sub-spaces grid
- /// </summary>
- /// <param name="geom">the pointer to the geom that moved</param>
- /// <param name="pos">the position that the geom moved to</param>
- /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
- /// <returns>a pointer to the new space it's in</returns>
- public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
- {
- // moves a prim into another static sub-space or from another space into a static sub-space
- // Called ODEPrim so
- // it's already in locked space.
- if (geom == IntPtr.Zero) // shouldn't happen
- return IntPtr.Zero;
- // get the static sub-space for current position
- IntPtr newspace = calculateSpaceForGeom(pos);
- if (newspace == currentspace) // if we are there all done
- return newspace;
- // else remove it from its current space
- if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
- {
- if (d.GeomIsSpace(currentspace))
- {
- waitForSpaceUnlock(currentspace);
- d.SpaceRemove(currentspace, geom);
- if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
- {
- d.SpaceDestroy(currentspace);
- }
- }
- else
- {
- m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
- " Geom:" + geom);
- }
- }
- else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
- {
- currentspace = d.GeomGetSpace(geom);
- if (currentspace != IntPtr.Zero)
- {
- if (d.GeomIsSpace(currentspace))
- {
- waitForSpaceUnlock(currentspace);
- d.SpaceRemove(currentspace, geom);
- if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
- {
- d.SpaceDestroy(currentspace);
- }
- }
- }
- }
- // put the geom in the newspace
- waitForSpaceUnlock(newspace);
- d.SpaceAdd(newspace, geom);
- // let caller know this newspace
- return newspace;
- }
- /// <summary>
- /// Calculates the space the prim should be in by its position
- /// </summary>
- /// <param name="pos"></param>
- /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
- public IntPtr calculateSpaceForGeom(Vector3 pos)
- {
- int x, y;
- if (pos.X < 0)
- return staticPrimspaceOffRegion[0];
- if (pos.Y < 0)
- return staticPrimspaceOffRegion[2];
- x = (int)(pos.X * spacesPerMeterX);
- if (x > spaceGridMaxX)
- return staticPrimspaceOffRegion[1];
- y = (int)(pos.Y * spacesPerMeterY);
- if (y > spaceGridMaxY)
- return staticPrimspaceOffRegion[3];
- return staticPrimspace[x, y];
- }
- #endregion
- /// <summary>
- /// Called to queue a change to a actor
- /// to use in place of old taint mechanism so changes do have a time sequence
- /// </summary>
- public void AddChange(PhysicsActor actor, changes what, Object arg)
- {
- ODEchangeitem item = new ODEchangeitem();
- item.actor = actor;
- item.what = what;
- item.arg = arg;
- ChangesQueue.Enqueue(item);
- }
- /// <summary>
- /// Called after our prim properties are set Scale, position etc.
- /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
- /// This assures us that we have no race conditions
- /// </summary>
- /// <param name="prim"></param>
- public override void AddPhysicsActorTaint(PhysicsActor prim)
- {
- }
- // does all pending changes generated during region load process
- public override void ProcessPreSimulation()
- {
- lock (OdeLock)
- {
- if (world == IntPtr.Zero)
- {
- ChangesQueue.Clear();
- return;
- }
- d.AllocateODEDataForThread(~0U);
- ODEchangeitem item;
- int donechanges = 0;
- if (ChangesQueue.Count > 0)
- {
- m_log.InfoFormat("[ubOde] start processing pending actor operations");
- int tstart = Util.EnvironmentTickCount();
- while (ChangesQueue.Dequeue(out item))
- {
- if (item.actor != null)
- {
- try
- {
- if (item.actor is OdeCharacter)
- ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
- else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
- RemovePrimThreadLocked((OdePrim)item.actor);
- }
- catch
- {
- m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
- item.actor.Name, item.what.ToString());
- }
- }
- donechanges++;
- }
- int time = Util.EnvironmentTickCountSubtract(tstart);
- m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time);
- }
- m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count);
- }
- }
- /// <summary>
- /// This is our main simulate loop
- /// It's thread locked by a Mutex in the scene.
- /// It holds Collisions, it instructs ODE to step through the physical reactions
- /// It moves the objects around in memory
- /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
- /// </summary>
- /// <param name="timeStep"></param>
- /// <returns></returns>
- public override float Simulate(float reqTimeStep)
- {
- DateTime now = DateTime.UtcNow;
- TimeSpan timedif = now - m_lastframe;
- float timeStep = (float)timedif.TotalSeconds;
- m_lastframe = now;
- // acumulate time so we can reduce error
- step_time += timeStep;
- if (step_time < HalfOdeStep)
- return 0;
- if (framecount < 0)
- framecount = 0;
- framecount++;
- // checkThread();
- int nodeframes = 0;
- float fps = 0;
- lock (SimulationLock)
- lock(OdeLock)
- {
- if (world == IntPtr.Zero)
- {
- ChangesQueue.Clear();
- return 0;
- }
- ODEchangeitem item;
- // d.WorldSetQuickStepNumIterations(world, curphysiteractions);
- double loopstartMS = Util.GetTimeStampMS();
- double looptimeMS = 0;
- double changestimeMS = 0;
- double maxChangestime = (int)(reqTimeStep * 500f); // half the time
- double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
- // double collisionTime = 0;
- // double qstepTIme = 0;
- // double tmpTime = 0;
- d.AllocateODEDataForThread(~0U);
- if (ChangesQueue.Count > 0)
- {
- while (ChangesQueue.Dequeue(out item))
- {
- if (item.actor != null)
- {
- try
- {
- if (item.actor is OdeCharacter)
- ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
- else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
- RemovePrimThreadLocked((OdePrim)item.actor);
- }
- catch
- {
- m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
- item.actor.Name, item.what.ToString());
- }
- }
- changestimeMS = Util.GetTimeStampMS() - loopstartMS;
- if (changestimeMS > maxChangestime)
- break;
- }
- }
- // do simulation taking at most 150ms total time including changes
- while (step_time > HalfOdeStep)
- {
- try
- {
- // clear pointer/counter to contacts to pass into joints
- m_global_contactcount = 0;
- // Move characters
- lock (_characters)
- {
- List<OdeCharacter> defects = new List<OdeCharacter>();
- foreach (OdeCharacter actor in _characters)
- {
- if (actor != null)
- actor.Move(defects);
- }
- if (defects.Count != 0)
- {
- foreach (OdeCharacter defect in defects)
- {
- RemoveCharacter(defect);
- }
- defects.Clear();
- }
- }
- // Move other active objects
- lock (_activegroups)
- {
- foreach (OdePrim aprim in _activegroups)
- {
- aprim.Move();
- }
- }
- m_rayCastManager.ProcessQueuedRequests();
- // tmpTime = Util.GetTimeStampMS();
- collision_optimized();
- // collisionTime += Util.GetTimeStampMS() - tmpTime;
- lock(_collisionEventPrimRemove)
- {
- foreach (PhysicsActor obj in _collisionEventPrimRemove)
- _collisionEventPrim.Remove(obj);
- _collisionEventPrimRemove.Clear();
- }
- List<OdePrim> sleepers = new List<OdePrim>();
- foreach (PhysicsActor obj in _collisionEventPrim)
- {
- if (obj == null)
- continue;
- switch ((ActorTypes)obj.PhysicsActorType)
- {
- case ActorTypes.Agent:
- OdeCharacter cobj = (OdeCharacter)obj;
- cobj.SendCollisions((int)(odetimestepMS));
- break;
- case ActorTypes.Prim:
- OdePrim pobj = (OdePrim)obj;
- if (!pobj.m_outbounds)
- {
- pobj.SendCollisions((int)(odetimestepMS));
- if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
- !pobj.m_disabled && !pobj.m_building &&
- !d.BodyIsEnabled(pobj.Body))
- sleepers.Add(pobj);
- }
- break;
- }
- }
- foreach(OdePrim prm in sleepers)
- prm.SleeperAddCollisionEvents();
- sleepers.Clear();
-
- // do a ode simulation step
- // tmpTime = Util.GetTimeStampMS();
- d.WorldQuickStep(world, ODE_STEPSIZE);
- d.JointGroupEmpty(contactgroup);
- // qstepTIme += Util.GetTimeStampMS() - tmpTime;
- // update managed ideia of physical data and do updates to core
- /*
- lock (_characters)
- {
- foreach (OdeCharacter actor in _characters)
- {
- if (actor != null)
- {
- if (actor.bad)
- m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
- actor.UpdatePositionAndVelocity();
- }
- }
- }
- */
- lock (_activegroups)
- {
- {
- foreach (OdePrim actor in _activegroups)
- {
- if (actor.IsPhysical)
- {
- actor.UpdatePositionAndVelocity(framecount);
- }
- }
- }
- }
- }
- catch (Exception e)
- {
- m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
- // ode.dunlock(world);
- }
- step_time -= ODE_STEPSIZE;
- nodeframes++;
- looptimeMS = Util.GetTimeStampMS() - loopstartMS;
- if (looptimeMS > maxLoopTime)
- break;
- }
- lock (_badCharacter)
- {
- if (_badCharacter.Count > 0)
- {
- foreach (OdeCharacter chr in _badCharacter)
- {
- RemoveCharacter(chr);
- }
- _badCharacter.Clear();
- }
- }
- timedif = now - m_lastMeshExpire;
- if (timedif.Seconds > 10)
- {
- mesher.ExpireReleaseMeshs();
- m_lastMeshExpire = now;
- }
- // information block for in debug breakpoint only
- /*
- int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
- int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
- int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
- int nactivegeoms = 0;
- int nactivespaces = 0;
- int nstaticgeoms = 0;
- int nstaticspaces = 0;
- IntPtr sp;
- for (int i = 0; i < ntopactivegeoms; i++)
- {
- sp = d.SpaceGetGeom(ActiveSpace, i);
- if (d.GeomIsSpace(sp))
- {
- nactivespaces++;
- nactivegeoms += d.SpaceGetNumGeoms(sp);
- }
- else
- nactivegeoms++;
- }
- for (int i = 0; i < ntopstaticgeoms; i++)
- {
- sp = d.SpaceGetGeom(StaticSpace, i);
- if (d.GeomIsSpace(sp))
- {
- nstaticspaces++;
- nstaticgeoms += d.SpaceGetNumGeoms(sp);
- }
- else
- nstaticgeoms++;
- }
- int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
- int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
- int nbodies = d.NTotalBodies;
- int ngeoms = d.NTotalGeoms;
- */
- /*
- looptimeMS /= nodeframes;
- if(looptimeMS > 0.080)
- {
- collisionTime /= nodeframes;
- qstepTIme /= nodeframes;
- }
- */
- // Finished with all sim stepping. If requested, dump world state to file for debugging.
- // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
- // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
- if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
- {
- string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
- string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
- if (physics_logging_append_existing_logfile)
- {
- string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
- TextWriter fwriter = File.AppendText(fname);
- fwriter.WriteLine(header);
- fwriter.Close();
- }
- d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
- }
- fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;
- if(step_time < HalfOdeStep)
- m_timeDilation = 1.0f;
- else if (step_time > m_SkipFramesAtms)
- {
- // if we lag too much skip frames
- m_timeDilation = 0.0f;
- step_time = 0;
- m_lastframe = DateTime.UtcNow; // skip also the time lost
- }
- else
- {
- m_timeDilation = ODE_STEPSIZE / step_time;
- if (m_timeDilation > 1)
- m_timeDilation = 1;
- }
- }
- return fps;
- }
- /// <summary>
- public override void GetResults()
- {
- }
- public override bool IsThreaded
- {
- // for now we won't be multithreaded
- get { return (false); }
- }
- public float GetTerrainHeightAtXY(float x, float y)
- {
- if (TerrainGeom == IntPtr.Zero)
- return 0f;
- if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
- return 0f;
- // TerrainHeightField for ODE as offset 1m
- x += 1f;
- y += 1f;
- // make position fit into array
- if (x < 0)
- x = 0;
- if (y < 0)
- y = 0;
- // integer indexs
- int ix;
- int iy;
- // interpolators offset
- float dx;
- float dy;
- int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
- int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
- int regsize = regsizeX;
- if (m_OSOdeLib)
- {
- if (x < regsizeX - 1)
- {
- ix = (int)x;
- dx = x - (float)ix;
- }
- else // out world use external height
- {
- ix = regsizeX - 2;
- dx = 0;
- }
- if (y < regsizeY - 1)
- {
- iy = (int)y;
- dy = y - (float)iy;
- }
- else
- {
- iy = regsizeY - 2;
- dy = 0;
- }
- }
- else
- {
- // we still have square fixed size regions
- // also flip x and y because of how map is done for ODE fliped axis
- // so ix,iy,dx and dy are inter exchanged
- regsize = regsizeY;
- if (x < regsizeX - 1)
- {
- iy = (int)x;
- dy = x - (float)iy;
- }
- else // out world use external height
- {
- iy = regsizeX - 2;
- dy = 0;
- }
- if (y < regsizeY - 1)
- {
- ix = (int)y;
- dx = y - (float)ix;
- }
- else
- {
- ix = regsizeY - 2;
- dx = 0;
- }
- }
- float h0;
- float h1;
- float h2;
- iy *= regsize;
- iy += ix; // all indexes have iy + ix
- float[] heights = TerrainHeightFieldHeight;
- /*
- if ((dx + dy) <= 1.0f)
- {
- h0 = ((float)heights[iy]); // 0,0 vertice
- h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
- h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
- }
- else
- {
- h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
- h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
- h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
- }
- */
- h0 = ((float)heights[iy]); // 0,0 vertice
- if (dy>dx)
- {
- iy += regsize;
- h2 = (float)heights[iy]; // 0,1 vertice
- h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
- h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
- }
- else
- {
- iy++;
- h2 = (float)heights[iy]; // vertice 1,0
- h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
- h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
- }
- return h0 + h1 + h2;
- }
- public Vector3 GetTerrainNormalAtXY(float x, float y)
- {
- Vector3 norm = new Vector3(0, 0, 1);
- if (TerrainGeom == IntPtr.Zero)
- return norm;
- if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
- return norm;
- // TerrainHeightField for ODE as offset 1m
- x += 1f;
- y += 1f;
- // make position fit into array
- if (x < 0)
- x = 0;
- if (y < 0)
- y = 0;
- // integer indexs
- int ix;
- int iy;
- // interpolators offset
- float dx;
- float dy;
- int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
- int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
- int regsize = regsizeX;
- int xstep = 1;
- int ystep = regsizeX;
- bool firstTri = false;
- if (m_OSOdeLib)
- {
- if (x < regsizeX - 1)
- {
- ix = (int)x;
- dx = x - (float)ix;
- }
- else // out world use external height
- {
- ix = regsizeX - 2;
- dx = 0;
- }
- if (y < regsizeY - 1)
- {
- iy = (int)y;
- dy = y - (float)iy;
- }
- else
- {
- iy = regsizeY - 2;
- dy = 0;
- }
- firstTri = dy > dx;
- }
- else
- {
- xstep = regsizeY;
- ystep = 1;
- regsize = regsizeY;
- // we still have square fixed size regions
- // also flip x and y because of how map is done for ODE fliped axis
- // so ix,iy,dx and dy are inter exchanged
- if (x < regsizeX - 1)
- {
- iy = (int)x;
- dy = x - (float)iy;
- }
- else // out world use external height
- {
- iy = regsizeX - 2;
- dy = 0;
- }
- if (y < regsizeY - 1)
- {
- ix = (int)y;
- dx = y - (float)ix;
- }
- else
- {
- ix = regsizeY - 2;
- dx = 0;
- }
- firstTri = dx > dy;
- }
- float h0;
- float h1;
- float h2;
- iy *= regsize;
- iy += ix; // all indexes have iy + ix
- float[] heights = TerrainHeightFieldHeight;
- if (firstTri)
- {
- h1 = ((float)heights[iy]); // 0,0 vertice
- iy += ystep;
- h0 = (float)heights[iy]; // 0,1
- h2 = (float)heights[iy+xstep]; // 1,1 vertice
- norm.X = h0 - h2;
- norm.Y = h1 - h0;
- }
- else
- {
- h2 = ((float)heights[iy]); // 0,0 vertice
- iy += xstep;
- h0 = ((float)heights[iy]); // 1,0 vertice
- h1 = (float)heights[iy+ystep]; // vertice 1,1
- norm.X = h2 - h0;
- norm.Y = h0 - h1;
- }
- norm.Z = 1;
- norm.Normalize();
- return norm;
- }
- public override void SetTerrain(float[] heightMap)
- {
- if (m_OSOdeLib)
- OSSetTerrain(heightMap);
- else
- OriSetTerrain(heightMap);
- }
- public void OriSetTerrain(float[] heightMap)
- {
- // assumes 1m size grid and constante size square regions
- // needs to know about sims around in future
- float[] _heightmap;
- uint regionsizeX = m_regionWidth;
- uint regionsizeY = m_regionHeight;
- // map is rotated
- uint heightmapWidth = regionsizeY + 2;
- uint heightmapHeight = regionsizeX + 2;
- uint heightmapWidthSamples = heightmapWidth + 1;
- uint heightmapHeightSamples = heightmapHeight + 1;
- _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
- const float scale = 1.0f;
- const float offset = 0.0f;
- const float thickness = 10f;
- const int wrap = 0;
- float hfmin = float.MaxValue;
- float hfmax = float.MinValue;
- float val;
- uint xx;
- uint yy;
- uint maxXX = regionsizeX - 1;
- uint maxYY = regionsizeY - 1;
- // flipping map adding one margin all around so things don't fall in edges
- uint xt = 0;
- xx = 0;
- for (uint x = 0; x < heightmapWidthSamples; x++)
- {
- if (x > 1 && xx < maxXX)
- xx++;
- yy = 0;
- for (uint y = 0; y < heightmapHeightSamples; y++)
- {
- if (y > 1 && y < maxYY)
- yy += regionsizeX;
- val = heightMap[yy + xx];
- if (val < 0.0f)
- val = 0.0f; // no neg terrain as in chode
- _heightmap[xt + y] = val;
- if (hfmin > val)
- hfmin = val;
- if (hfmax < val)
- hfmax = val;
- }
- xt += heightmapHeightSamples;
- }
- lock (OdeLock)
- {
- d.AllocateODEDataForThread(~0U);
- if (TerrainGeom != IntPtr.Zero)
- {
- actor_name_map.Remove(TerrainGeom);
- d.GeomDestroy(TerrainGeom);
- }
- if (TerrainHeightFieldHeightsHandler.IsAllocated)
- TerrainHeightFieldHeightsHandler.Free();
- IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
- TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
- d.GeomHeightfieldDataBuildSingle(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0,
- heightmapHeight, heightmapWidth ,
- (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
- offset, thickness, wrap);
- d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
- TerrainGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
- if (TerrainGeom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
- d.GeomSetCollideBits(TerrainGeom, 0);
- PhysicsActor pa = new NullPhysicsActor();
- pa.Name = "Terrain";
- pa.PhysicsActorType = (int)ActorTypes.Ground;
- actor_name_map[TerrainGeom] = pa;
- // geom_name_map[GroundGeom] = "Terrain";
- d.Quaternion q = new d.Quaternion();
- q.X = 0.5f;
- q.Y = 0.5f;
- q.Z = 0.5f;
- q.W = 0.5f;
- d.GeomSetQuaternion(TerrainGeom, ref q);
- d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
- TerrainHeightFieldHeight = _heightmap;
- }
- else
- TerrainHeightFieldHeightsHandler.Free();
- }
- }
- public void OSSetTerrain(float[] heightMap)
- {
- // assumes 1m size grid and constante size square regions
- // needs to know about sims around in future
- float[] _heightmap;
- uint regionsizeX = m_regionWidth;
- uint regionsizeY = m_regionHeight;
- uint heightmapWidth = regionsizeX + 2;
- uint heightmapHeight = regionsizeY + 2;
- uint heightmapWidthSamples = heightmapWidth + 1;
- uint heightmapHeightSamples = heightmapHeight + 1;
- _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
- float hfmin = float.MaxValue;
- // float hfmax = float.MinValue;
- float val;
- uint maxXX = regionsizeX - 1;
- uint maxYY = regionsizeY - 1;
- // adding one margin all around so things don't fall in edges
- uint xx;
- uint yy = 0;
- uint yt = 0;
- for (uint y = 0; y < heightmapHeightSamples; y++)
- {
- if (y > 1 && y < maxYY)
- yy += regionsizeX;
- xx = 0;
- for (uint x = 0; x < heightmapWidthSamples; x++)
- {
- if (x > 1 && x < maxXX)
- xx++;
- val = heightMap[yy + xx];
- if (val < 0.0f)
- val = 0.0f; // no neg terrain as in chode
- _heightmap[yt + x] = val;
- if (hfmin > val)
- hfmin = val;
- // if (hfmax < val)
- // hfmax = val;
- }
- yt += heightmapWidthSamples;
- }
- lock (OdeLock)
- {
- if (TerrainGeom != IntPtr.Zero)
- {
- actor_name_map.Remove(TerrainGeom);
- d.GeomDestroy(TerrainGeom);
- }
- if (TerrainHeightFieldHeightsHandler.IsAllocated)
- TerrainHeightFieldHeightsHandler.Free();
- TerrainHeightFieldHeight = null;
- IntPtr HeightmapData = d.GeomOSTerrainDataCreate();
- const int wrap = 0;
- float thickness = hfmin;
- if (thickness < 0)
- thickness = 1;
- TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
- d.GeomOSTerrainDataBuild(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
- (int)heightmapWidthSamples, (int)heightmapHeightSamples,
- thickness, wrap);
- // d.GeomOSTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
- TerrainGeom = d.CreateOSTerrain(GroundSpace, HeightmapData, 1);
- if (TerrainGeom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
- d.GeomSetCollideBits(TerrainGeom, 0);
- PhysicsActor pa = new NullPhysicsActor();
- pa.Name = "Terrain";
- pa.PhysicsActorType = (int)ActorTypes.Ground;
- actor_name_map[TerrainGeom] = pa;
- // geom_name_map[GroundGeom] = "Terrain";
- d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
- TerrainHeightFieldHeight = _heightmap;
- }
- else
- TerrainHeightFieldHeightsHandler.Free();
- }
- }
- public override void DeleteTerrain()
- {
- }
- public float GetWaterLevel()
- {
- return waterlevel;
- }
- public override void SetWaterLevel(float baseheight)
- {
- waterlevel = baseheight;
- }
- public override void Dispose()
- {
- lock (OdeLock)
- {
- if (world == IntPtr.Zero)
- return;
- d.AllocateODEDataForThread(~0U);
- if (m_meshWorker != null)
- m_meshWorker.Stop();
- if (m_rayCastManager != null)
- {
- m_rayCastManager.Dispose();
- m_rayCastManager = null;
- }
- lock (_prims)
- {
- ChangesQueue.Clear();
- foreach (OdePrim prm in _prims.Values)
- {
- prm.DoAChange(changes.Remove, null);
- _collisionEventPrim.Remove(prm);
- }
- _prims.Clear();
- }
- OdeCharacter[] chtorem;
- lock (_characters)
- {
- chtorem = new OdeCharacter[_characters.Count];
- _characters.CopyTo(chtorem);
- }
- ChangesQueue.Clear();
- foreach (OdeCharacter ch in chtorem)
- ch.DoAChange(changes.Remove, null);
- if (TerrainGeom != IntPtr.Zero)
- d.GeomDestroy(TerrainGeom);
- TerrainGeom = IntPtr.Zero;
- if (TerrainHeightFieldHeightsHandler.IsAllocated)
- TerrainHeightFieldHeightsHandler.Free();
- TerrainHeightFieldHeight = null;
- if (ContactgeomsArray != IntPtr.Zero)
- {
- Marshal.FreeHGlobal(ContactgeomsArray);
- ContactgeomsArray = IntPtr.Zero;
- }
- if (GlobalContactsArray != IntPtr.Zero)
- {
- Marshal.FreeHGlobal(GlobalContactsArray);
- GlobalContactsArray = IntPtr.Zero;
- }
- d.WorldDestroy(world);
- world = IntPtr.Zero;
- //d.CloseODE();
- }
- }
- private int compareByCollisionsDesc(OdePrim A, OdePrim B)
- {
- return -A.CollisionScore.CompareTo(B.CollisionScore);
- }
- public override Dictionary<uint, float> GetTopColliders()
- {
- Dictionary<uint, float> topColliders;
- List<OdePrim> orderedPrims;
- lock (_activeprims)
- orderedPrims = new List<OdePrim>(_activeprims);
- orderedPrims.Sort(compareByCollisionsDesc);
- topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
- return topColliders;
- }
- public override bool SupportsRayCast()
- {
- return true;
- }
- public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
- {
- if (retMethod != null)
- {
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.length = length;
- req.Normal = direction;
- req.Origin = position;
- req.Count = 0;
- req.filter = RayFilterFlags.AllPrims;
- m_rayCastManager.QueueRequest(req);
- }
- }
- public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
- {
- if (retMethod != null)
- {
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.length = length;
- req.Normal = direction;
- req.Origin = position;
- req.Count = Count;
- req.filter = RayFilterFlags.AllPrims;
- m_rayCastManager.QueueRequest(req);
- }
- }
- public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
- {
- List<ContactResult> ourresults = new List<ContactResult>();
- object SyncObject = new object();
- RayCallback retMethod = delegate(List<ContactResult> results)
- {
- lock (SyncObject)
- {
- ourresults = results;
- Monitor.PulseAll(SyncObject);
- }
- };
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.length = length;
- req.Normal = direction;
- req.Origin = position;
- req.Count = Count;
- req.filter = RayFilterFlags.AllPrims;
- lock (SyncObject)
- {
- m_rayCastManager.QueueRequest(req);
- if (!Monitor.Wait(SyncObject, 500))
- return null;
- else
- return ourresults;
- }
- }
- public override bool SupportsRaycastWorldFiltered()
- {
- return true;
- }
- public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
- {
- object SyncObject = new object();
- List<ContactResult> ourresults = new List<ContactResult>();
- RayCallback retMethod = delegate(List<ContactResult> results)
- {
- lock (SyncObject)
- {
- ourresults = results;
- Monitor.PulseAll(SyncObject);
- }
- };
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.length = length;
- req.Normal = direction;
- req.Origin = position;
- req.Count = Count;
- req.filter = filter;
- lock (SyncObject)
- {
- m_rayCastManager.QueueRequest(req);
- if (!Monitor.Wait(SyncObject, 500))
- return null;
- else
- return ourresults;
- }
- }
- public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
- {
- if (actor == null)
- return new List<ContactResult>();
- IntPtr geom;
- if (actor is OdePrim)
- geom = ((OdePrim)actor).prim_geom;
- else if (actor is OdeCharacter)
- geom = ((OdePrim)actor).prim_geom;
- else
- return new List<ContactResult>();
- if (geom == IntPtr.Zero)
- return new List<ContactResult>();
- List<ContactResult> ourResults = null;
- object SyncObject = new object();
- RayCallback retMethod = delegate(List<ContactResult> results)
- {
- lock (SyncObject)
- {
- ourResults = results;
- Monitor.PulseAll(SyncObject);
- }
- };
- ODERayRequest req = new ODERayRequest();
- req.actor = actor;
- req.callbackMethod = retMethod;
- req.length = length;
- req.Normal = direction;
- req.Origin = position;
- req.Count = Count;
- req.filter = flags;
- lock (SyncObject)
- {
- m_rayCastManager.QueueRequest(req);
- if (!Monitor.Wait(SyncObject, 500))
- return new List<ContactResult>();
- }
- if (ourResults == null)
- return new List<ContactResult>();
- return ourResults;
- }
- public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
- {
- List<ContactResult> ourResults = null;
- object SyncObject = new object();
- ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
- {
- lock (SyncObject)
- {
- ourResults = results;
- Monitor.PulseAll(SyncObject);
- }
- };
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.Normal = size;
- req.Origin = position;
- req.orientation = orientation;
- req.Count = Count;
- req.filter = flags;
- lock (SyncObject)
- {
- m_rayCastManager.QueueRequest(req);
- if (!Monitor.Wait(SyncObject, 500))
- return new List<ContactResult>();
- }
- if (ourResults == null)
- return new List<ContactResult>();
- return ourResults;
- }
- public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
- {
- List<ContactResult> ourResults = null;
- object SyncObject = new object();
- ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
- {
- ourResults = results;
- Monitor.PulseAll(SyncObject);
- };
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.length = radius;
- req.Origin = position;
- req.Count = Count;
- req.filter = flags;
- lock (SyncObject)
- {
- m_rayCastManager.QueueRequest(req);
- if (!Monitor.Wait(SyncObject, 500))
- return new List<ContactResult>();
- }
- if (ourResults == null)
- return new List<ContactResult>();
- return ourResults;
- }
- public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
- {
- IntPtr geom = IntPtr.Zero;;
- if (actor != null)
- {
- if (actor is OdePrim)
- geom = ((OdePrim)actor).prim_geom;
- else if (actor is OdeCharacter)
- geom = ((OdePrim)actor).prim_geom;
- }
- List<ContactResult> ourResults = null;
- object SyncObject = new object();
- ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
- {
- ourResults = results;
- Monitor.PulseAll(SyncObject);
- };
- ODERayRequest req = new ODERayRequest();
- req.actor = null;
- req.callbackMethod = retMethod;
- req.length = plane.W;
- req.Normal.X = plane.X;
- req.Normal.Y = plane.Y;
- req.Normal.Z = plane.Z;
- req.Count = Count;
- req.filter = flags;
- lock (SyncObject)
- {
- m_rayCastManager.QueueRequest(req);
- if (!Monitor.Wait(SyncObject, 500))
- return new List<ContactResult>();
- }
- if (ourResults == null)
- return new List<ContactResult>();
- return ourResults;
- }
- public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
- {
- Util.FireAndForget( delegate
- {
- ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
- if(sitAvatar != null)
- sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
- });
- return 1;
- }
- }
- }
|