1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728 |
- /*
- * 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 OpenSim 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.
- */
- using System;
- using System.Collections.Generic;
- using System.Reflection;
- using System.Runtime.InteropServices;
- using System.Threading;
- using log4net;
- using OpenMetaverse;
- using Ode.NET;
- using OpenSim.Framework;
- using OpenSim.Region.Physics.Manager;
- namespace OpenSim.Region.Physics.OdePlugin
- {
- /// <summary>
- /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
- /// </summary>
- public class OdePrim : PhysicsActor
- {
- private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
- public PhysicsVector _position;
- private PhysicsVector _velocity;
- private PhysicsVector _torque = new PhysicsVector(0,0,0);
- private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);
- private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f);
- private Quaternion m_lastorientation = new Quaternion();
- private PhysicsVector m_rotationalVelocity;
- private PhysicsVector _size;
- private PhysicsVector _acceleration;
- // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
- private Quaternion _orientation;
- private PhysicsVector m_taintposition;
- private PhysicsVector m_taintsize;
- private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0);
- private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0);
- private Quaternion m_taintrot;
- private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f);
- private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f);
- private IntPtr Amotor = IntPtr.Zero;
- private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
- private float m_PIDTau = 0f;
- private float PID_D = 35f;
- private float PID_G = 25f;
- private float m_tensor = 5f;
- private int body_autodisable_frames = 20;
- private IMesh primMesh = null;
- private bool m_usePID = false;
- private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
- | CollisionCategories.Space
- | CollisionCategories.Body
- | CollisionCategories.Character
- );
- private bool m_taintshape = false;
- private bool m_taintPhysics = false;
- private bool m_collidesLand = true;
- private bool m_collidesWater = false;
- public bool m_returnCollisions = false;
- // Default we're a Geometry
- private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
- // Default, Collide with Other Geometries, spaces and Bodies
- private CollisionCategories m_collisionFlags = m_default_collisionFlags;
- public bool m_taintremove = false;
- public bool m_taintdisable = false;
- public bool m_disabled = false;
- public bool m_taintadd = false;
- public bool m_taintselected = false;
- public bool m_taintCollidesWater = false;
- public uint m_localID = 0;
- //public GCHandle gc;
- private CollisionLocker ode;
- private bool m_taintforce = false;
- private bool m_taintaddangularforce = false;
- private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f);
- private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();
- private List<PhysicsVector> m_angularforcelist = new List<PhysicsVector>();
- private IMesh _mesh;
- private PrimitiveBaseShape _pbs;
- private OdeScene _parent_scene;
- public IntPtr m_targetSpace = (IntPtr) 0;
- public IntPtr prim_geom;
- public IntPtr prev_geom;
- public IntPtr _triMeshData;
- private IntPtr _linkJointGroup = (IntPtr)0;
- private PhysicsActor _parent = null;
- private PhysicsActor m_taintparent = null;
- private List<OdePrim> childrenPrim = new List<OdePrim>();
- private bool iscolliding = false;
- private bool m_isphysical = false;
- private bool m_isSelected = false;
- internal bool m_isVolumeDetect = false; // If true, this prim only detects collisions but doesn't collide actively
- private bool m_throttleUpdates = false;
- private int throttleCounter = 0;
- public int m_interpenetrationcount = 0;
- public float m_collisionscore = 0;
- public int m_roundsUnderMotionThreshold = 0;
- private int m_crossingfailures = 0;
- public float m_buoyancy = 0f;
- public bool outofBounds = false;
- private float m_density = 10.000006836f; // Aluminum g/cm3;
- public bool _zeroFlag = false;
- private bool m_lastUpdateSent = false;
- public IntPtr Body = (IntPtr) 0;
- private String m_primName;
- private PhysicsVector _target_velocity;
- public d.Mass pMass;
- public int m_eventsubscription = 0;
- private CollisionEventUpdate CollisionEventsThisFrame = null;
- private IntPtr m_linkJoint = (IntPtr)0;
- public volatile bool childPrim = false;
- public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
- Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
- {
- _target_velocity = new PhysicsVector(0, 0, 0);
- //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
- ode = dode;
- _velocity = new PhysicsVector();
- _position = pos;
- m_taintposition = pos;
- PID_D = parent_scene.bodyPIDD;
- PID_G = parent_scene.bodyPIDG;
- m_density = parent_scene.geomDefaultDensity;
- m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
- body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
- prim_geom = IntPtr.Zero;
- prev_geom = IntPtr.Zero;
- if (size.X <= 0) size.X = 0.01f;
- if (size.Y <= 0) size.Y = 0.01f;
- if (size.Z <= 0) size.Z = 0.01f;
- _size = size;
- m_taintsize = _size;
- _acceleration = new PhysicsVector();
- m_rotationalVelocity = PhysicsVector.Zero;
- _orientation = rotation;
- m_taintrot = _orientation;
- _mesh = mesh;
- _pbs = pbs;
- _parent_scene = parent_scene;
- m_targetSpace = (IntPtr)0;
- if (pos.Z < 0)
- m_isphysical = false;
- else
- {
- m_isphysical = pisPhysical;
- // If we're physical, we need to be in the master space for now.
- // linksets *should* be in a space together.. but are not currently
- if (m_isphysical)
- m_targetSpace = _parent_scene.space;
- }
- m_primName = primName;
- m_taintadd = true;
- _parent_scene.AddPhysicsActorTaint(this);
- // don't do .add() here; old geoms get recycled with the same hash
- }
- public override int PhysicsActorType
- {
- get { return (int) ActorTypes.Prim; }
- set { return; }
- }
- public override bool SetAlwaysRun
- {
- get { return false; }
- set { return; }
- }
- public override uint LocalID
- {
- set {
- //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
- m_localID = value; }
- }
- public override bool Grabbed
- {
- set { return; }
- }
- public override bool Selected
- {
- set {
- // This only makes the object not collidable if the object
- // is physical or the object is modified somehow *IN THE FUTURE*
- // without this, if an avatar selects prim, they can walk right
- // through it while it's selected
- m_collisionscore = 0;
- if ((m_isphysical && !_zeroFlag) || !value)
- {
- m_taintselected = value;
- _parent_scene.AddPhysicsActorTaint(this);
- }
- else
- {
- m_taintselected = value;
- m_isSelected = value;
- }
- }
- }
- public void SetGeom(IntPtr geom)
- {
- prev_geom = prim_geom;
- prim_geom = geom;
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- }
- if (childPrim)
- {
- if (_parent != null && _parent is OdePrim)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildSetGeom(this);
- }
- }
- //m_log.Warn("Setting Geom to: " + prim_geom);
- }
-
- public void enableBodySoft()
- {
- if (!childPrim)
- {
- if (m_isphysical && Body != IntPtr.Zero)
- d.BodyEnable(Body);
- m_disabled = false;
- }
- }
- public void disableBodySoft()
- {
- m_disabled = true;
- if (m_isphysical && Body != IntPtr.Zero)
- d.BodyDisable(Body);
- }
- public void enableBody()
- {
- // Don't enable this body if we're a child prim
- // this should be taken care of in the parent function not here
- if (!childPrim)
- {
- // Sets the geom to a body
- Body = d.BodyCreate(_parent_scene.world);
- setMass();
- d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
- d.Quaternion myrot = new d.Quaternion();
- myrot.X = _orientation.X;
- myrot.Y = _orientation.Y;
- myrot.Z = _orientation.Z;
- myrot.W = _orientation.W;
- d.BodySetQuaternion(Body, ref myrot);
- d.GeomSetBody(prim_geom, Body);
- m_collisionCategories |= CollisionCategories.Body;
- m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- 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.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
- {
- createAMotor(m_angularlock);
- }
- _parent_scene.addActivePrim(this);
- }
- }
- #region Mass Calculation
- private float CalculateMass()
- {
- float volume = 0;
- // No material is passed to the physics engines yet.. soo..
- // we're using the m_density constant in the class definition
- float returnMass = 0;
- switch (_pbs.ProfileShape)
- {
- case ProfileShape.Square:
- // Profile Volume
- volume = _size.X*_size.Y*_size.Z;
- // If the user has 'hollowed out'
- // ProfileHollow is one of those 0 to 50000 values :P
- // we like percentages better.. so turning into a percentage
- if (((float) _pbs.ProfileHollow/50000f) > 0.0)
- {
- float hollowAmount = (float) _pbs.ProfileHollow/50000f;
- // calculate the hollow volume by it's shape compared to the prim shape
- float hollowVolume = 0;
- switch (_pbs.HollowShape)
- {
- case HollowShape.Square:
- case HollowShape.Same:
- // Cube Hollow volume calculation
- float hollowsizex = _size.X*hollowAmount;
- float hollowsizey = _size.Y*hollowAmount;
- float hollowsizez = _size.Z*hollowAmount;
- hollowVolume = hollowsizex*hollowsizey*hollowsizez;
- break;
- case HollowShape.Circle:
- // Hollow shape is a perfect cyllinder in respect to the cube's scale
- // Cyllinder hollow volume calculation
- float hRadius = _size.X/2;
- float hLength = _size.Z;
- // pi * r2 * h
- hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
- break;
- case HollowShape.Triangle:
- // Equilateral Triangular Prism volume hollow calculation
- // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
- float aLength = _size.Y;
- // 1/2 abh
- hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
- break;
- default:
- hollowVolume = 0;
- break;
- }
- volume = volume - hollowVolume;
- }
- break;
- case ProfileShape.Circle:
- if (_pbs.PathCurve == (byte)Extrusion.Straight)
- {
- // Cylinder
- float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
- float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
- // Approximating the cylinder's irregularity.
- if (volume1 > volume2)
- {
- volume = (float)volume1 - (volume1 - volume2);
- }
- else if (volume2 > volume1)
- {
- volume = (float)volume2 - (volume2 - volume1);
- }
- else
- {
- // Regular cylinder
- volume = volume1;
- }
- }
- else
- {
- // We don't know what the shape is yet, so use default
- volume = _size.X * _size.Y * _size.Z;
- }
- // If the user has 'hollowed out'
- // ProfileHollow is one of those 0 to 50000 values :P
- // we like percentages better.. so turning into a percentage
- if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
- {
- float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
- // calculate the hollow volume by it's shape compared to the prim shape
- float hollowVolume = 0;
- switch (_pbs.HollowShape)
- {
- case HollowShape.Same:
- case HollowShape.Circle:
- // Hollow shape is a perfect cyllinder in respect to the cube's scale
- // Cyllinder hollow volume calculation
- float hRadius = _size.X / 2;
- float hLength = _size.Z;
- // pi * r2 * h
- hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
- break;
- case HollowShape.Square:
- // Cube Hollow volume calculation
- float hollowsizex = _size.X * hollowAmount;
- float hollowsizey = _size.Y * hollowAmount;
- float hollowsizez = _size.Z * hollowAmount;
- hollowVolume = hollowsizex * hollowsizey * hollowsizez;
- break;
- case HollowShape.Triangle:
- // Equilateral Triangular Prism volume hollow calculation
- // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
- float aLength = _size.Y;
- // 1/2 abh
- hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
- break;
- default:
- hollowVolume = 0;
- break;
- }
- volume = volume - hollowVolume;
- }
- break;
- case ProfileShape.HalfCircle:
- if (_pbs.PathCurve == (byte)Extrusion.Curve1)
- {
- if (_size.X == _size.Z && _size.Z == _size.X)
- {
- // regular sphere
- // v = 4/3 * pi * r^3
- float sradius3 = (float)Math.Pow((_size.X / 2), 3);
- volume = (float)((4 / 3) * Math.PI * sradius3);
- }
- else
- {
- // we treat this as a box currently
- volume = _size.X * _size.Y * _size.Z;
- }
- }
- else
- {
- // We don't know what the shape is yet, so use default
- volume = _size.X * _size.Y * _size.Z;
- }
- break;
- case ProfileShape.EquilateralTriangle:
- /*
- v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
- // seed mesh
- Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
- Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
- Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
- */
- float xA = -0.25f * _size.X;
- float yA = -0.45f * _size.Y;
- float xB = 0.5f * _size.X;
- float yB = 0;
- float xC = -0.25f * _size.X;
- float yC = 0.45f * _size.Y;
- volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
- // If the user has 'hollowed out'
- // ProfileHollow is one of those 0 to 50000 values :P
- // we like percentages better.. so turning into a percentage
- float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
- if (((float)fhollowFactor / 50000f) > 0.0)
- {
- float hollowAmount = (float)fhollowFactor / 50000f;
- // calculate the hollow volume by it's shape compared to the prim shape
- float hollowVolume = 0;
- switch (_pbs.HollowShape)
- {
- case HollowShape.Same:
- case HollowShape.Triangle:
- // Equilateral Triangular Prism volume hollow calculation
- // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
- float aLength = _size.Y;
- // 1/2 abh
- hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
- break;
- case HollowShape.Square:
- // Cube Hollow volume calculation
- float hollowsizex = _size.X * hollowAmount;
- float hollowsizey = _size.Y * hollowAmount;
- float hollowsizez = _size.Z * hollowAmount;
- hollowVolume = hollowsizex * hollowsizey * hollowsizez;
- break;
- case HollowShape.Circle:
- // Hollow shape is a perfect cyllinder in respect to the cube's scale
- // Cyllinder hollow volume calculation
- float hRadius = _size.X / 2;
- float hLength = _size.Z;
- // pi * r2 * h
- hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
- break;
- default:
- hollowVolume = 0;
- break;
- }
- volume = volume - hollowVolume;
- }
- break;
- default:
- // we don't have all of the volume formulas yet so
- // use the common volume formula for all
- volume = _size.X*_size.Y*_size.Z;
- break;
- }
- // Calculate Path cut effect on volume
- // Not exact, in the triangle hollow example
- // They should never be zero or less then zero..
- // we'll ignore it if it's less then zero
- // ProfileEnd and ProfileBegin are values
- // from 0 to 50000
- // Turning them back into percentages so that I can cut that percentage off the volume
- float PathCutEndAmount = _pbs.ProfileEnd;
- float PathCutStartAmount = _pbs.ProfileBegin;
- if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
- {
- float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
- // Check the return amount for sanity
- if (pathCutAmount >= 0.99f)
- pathCutAmount = 0.99f;
- volume = volume - (volume*pathCutAmount);
- }
- UInt16 taperX = _pbs.PathScaleX;
- UInt16 taperY = _pbs.PathScaleY;
- float taperFactorX = 0;
- float taperFactorY = 0;
- // Mass = density * volume
- if (taperX != 100)
- {
- if (taperX > 100)
- {
- taperFactorX = 1.0f - ((float)taperX / 200);
- //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
- }
- else
- {
- taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
- //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
- }
- volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
- }
- if (taperY != 100)
- {
- if (taperY > 100)
- {
- taperFactorY = 1.0f - ((float)taperY / 200);
- //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
- }
- else
- {
- taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
- //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
- }
- volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
- }
- returnMass = m_density*volume;
- if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
- // Recursively calculate mass
- bool HasChildPrim = false;
- lock (childrenPrim)
- {
- if (childrenPrim.Count > 0)
- {
- HasChildPrim = true;
- }
- }
- if (HasChildPrim)
- {
- OdePrim[] childPrimArr = new OdePrim[0];
- lock (childrenPrim)
- childPrimArr = childrenPrim.ToArray();
- for (int i = 0; i < childPrimArr.Length; i++)
- {
- if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
- returnMass += childPrimArr[i].CalculateMass();
- // failsafe, this shouldn't happen but with OpenSim, you never know :)
- if (i > 256)
- break;
- }
- }
-
-
- return returnMass;
- }
- #endregion
- public void setMass()
- {
- if (Body != (IntPtr) 0)
- {
- float newmass = CalculateMass();
- //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
- d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
- d.BodySetMass(Body, ref pMass);
- }
- }
- public void disableBody()
- {
- //this kills the body so things like 'mesh' can re-create it.
- lock (this)
- {
- if (!childPrim)
- {
- if (Body != IntPtr.Zero)
- {
- _parent_scene.remActivePrim(this);
-
- m_collisionCategories &= ~CollisionCategories.Body;
- m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- }
-
- d.BodyDestroy(Body);
- lock (childrenPrim)
- {
- if (childrenPrim.Count > 0)
- {
- foreach (OdePrim prm in childrenPrim)
- {
- _parent_scene.remActivePrim(prm);
- prm.Body = IntPtr.Zero;
- }
- }
- }
- Body = IntPtr.Zero;
- }
- }
- else
- {
- _parent_scene.remActivePrim(this);
-
- m_collisionCategories &= ~CollisionCategories.Body;
- m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- }
-
- Body = IntPtr.Zero;
- }
- }
- m_disabled = true;
- m_collisionscore = 0;
- }
- public void setMesh(OdeScene parent_scene, IMesh mesh)
- {
- // This sleeper is there to moderate how long it takes between
- // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
- Thread.Sleep(10);
- //Kill Body so that mesh can re-make the geom
- if (IsPhysical && Body != IntPtr.Zero)
- {
- if (childPrim)
- {
- if (_parent != null)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildDelink(this);
- }
- }
- else
- {
- disableBody();
- }
- }
- IMesh oldMesh = primMesh;
- primMesh = mesh;
- float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
- int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
- primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory
- int VertexCount = vertexList.GetLength(0)/3;
- int IndexCount = indexList.GetLength(0);
- _triMeshData = d.GeomTriMeshDataCreate();
- d.GeomTriMeshDataBuildSimple(_triMeshData, vertexList, 3*sizeof (float), VertexCount, indexList, IndexCount,
- 3*sizeof (int));
- d.GeomTriMeshDataPreprocess(_triMeshData);
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- try
- {
- if (prim_geom == IntPtr.Zero)
- {
- SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
- }
- }
- catch (AccessViolationException)
- {
- m_log.Error("[PHYSICS]: MESH LOCKED");
- return;
- }
- if (oldMesh != null)
- {
- oldMesh.releasePinned();
- oldMesh = null;
- }
- // if (IsPhysical && Body == (IntPtr) 0)
- // {
- // Recreate the body
- // m_interpenetrationcount = 0;
- // m_collisionscore = 0;
- // enableBody();
- // }
- }
- public void ProcessTaints(float timestep)
- {
- if (m_taintadd)
- {
- changeadd(timestep);
- }
- if (prim_geom != IntPtr.Zero)
- {
- if (!_position.IsIdentical(m_taintposition,0f))
- changemove(timestep);
- if (m_taintrot != _orientation)
- rotate(timestep);
- //
- if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
- changePhysicsStatus(timestep);
- //
- if (!_size.IsIdentical(m_taintsize,0))
- changesize(timestep);
- //
- if (m_taintshape)
- changeshape(timestep);
- //
- if (m_taintforce)
- changeAddForce(timestep);
- if (m_taintaddangularforce)
- changeAddAngularForce(timestep);
- if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f))
- changeSetTorque(timestep);
- if (m_taintdisable)
- changedisable(timestep);
- if (m_taintselected != m_isSelected)
- changeSelectedStatus(timestep);
- if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero, 0.001f))
- changevelocity(timestep);
- if (m_taintparent != _parent)
- changelink(timestep);
- if (m_taintCollidesWater != m_collidesWater)
- changefloatonwater(timestep);
- if (!m_angularlock.IsIdentical(m_taintAngularLock,0))
- changeAngularLock(timestep);
- }
- else
- {
- m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)");
- }
- }
- private void changeAngularLock(float timestep)
- {
- // do we have a Physical object?
- if (Body != IntPtr.Zero)
- {
- //Check that we have a Parent
- //If we have a parent then we're not authorative here
- if (_parent == null)
- {
- if (!m_taintAngularLock.IsIdentical(new PhysicsVector(1f,1f,1f), 0))
- {
- //d.BodySetFiniteRotationMode(Body, 0);
- //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
- createAMotor(m_taintAngularLock);
- }
- else
- {
- if (Amotor != IntPtr.Zero)
- {
- d.JointDestroy(Amotor);
- Amotor = IntPtr.Zero;
- }
- }
- }
- }
- // Store this for later in case we get turned into a separate body
- m_angularlock = new PhysicsVector(m_taintAngularLock.X, m_taintAngularLock.Y, m_taintAngularLock.Z);
- }
- private void changelink(float timestep)
- {
- // If the newly set parent is not null
- // create link
- if (_parent == null && m_taintparent != null)
- {
- if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
- {
- OdePrim obj = (OdePrim)m_taintparent;
- //obj.disableBody();
- obj.ParentPrim(this);
- /*
- if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
- {
- _linkJointGroup = d.JointGroupCreate(0);
- m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
- d.JointAttach(m_linkJoint, obj.Body, Body);
- d.JointSetFixed(m_linkJoint);
- }
- */
- }
- }
- // If the newly set parent is null
- // destroy link
- else if (_parent != null && m_taintparent == null)
- {
- if (_parent is OdePrim)
- {
- OdePrim obj = (OdePrim)_parent;
- obj.ChildDelink(this);
- childPrim = false;
- //_parent = null;
- }
-
- /*
- if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
- d.JointGroupDestroy(_linkJointGroup);
-
- _linkJointGroup = (IntPtr)0;
- m_linkJoint = (IntPtr)0;
- */
- }
- _parent = m_taintparent;
- m_taintPhysics = m_isphysical;
- }
- // I'm the parent
- // prim is the child
- public void ParentPrim(OdePrim prim)
- {
- if (this.m_localID != prim.m_localID)
- {
- if (Body == IntPtr.Zero)
- {
- Body = d.BodyCreate(_parent_scene.world);
- }
- if (Body != IntPtr.Zero)
- {
- lock (childrenPrim)
- {
- if (!childrenPrim.Contains(prim))
- {
- 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(out m2, ref mat);
- d.MassTranslate(out 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);
- if (prm.prim_geom == IntPtr.Zero)
- {
- m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
- continue;
- }
- 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(out 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.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
- {
- prm.createAMotor(m_angularlock);
- }
- prm.Body = Body;
- _parent_scene.addActivePrim(prm);
- }
- m_collisionCategories |= CollisionCategories.Body;
- m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- 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(out 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.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
- {
- createAMotor(m_angularlock);
- }
- d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
- _parent_scene.addActivePrim(this);
- }
- }
- }
- }
- }
- private void ChildSetGeom(OdePrim odePrim)
- {
- //if (m_isphysical && Body != IntPtr.Zero)
- lock (childrenPrim)
- {
- foreach (OdePrim prm in childrenPrim)
- {
- //prm.childPrim = true;
- prm.disableBody();
- //prm.m_taintparent = null;
- //prm._parent = null;
- //prm.m_taintPhysics = false;
- //prm.m_disabled = true;
- //prm.childPrim = false;
- }
- }
- disableBody();
- if (Body != IntPtr.Zero)
- {
- _parent_scene.remActivePrim(this);
- }
- lock (childrenPrim)
- {
- foreach (OdePrim prm in childrenPrim)
- {
- ParentPrim(prm);
- }
- }
-
- }
- private void ChildDelink(OdePrim odePrim)
- {
- // Okay, we have a delinked child.. need to rebuild the body.
- lock (childrenPrim)
- {
- foreach (OdePrim prm in childrenPrim)
- {
- prm.childPrim = true;
- prm.disableBody();
- //prm.m_taintparent = null;
- //prm._parent = null;
- //prm.m_taintPhysics = false;
- //prm.m_disabled = true;
- //prm.childPrim = false;
- }
- }
- disableBody();
- lock (childrenPrim)
- {
- childrenPrim.Remove(odePrim);
- }
-
-
-
- if (Body != IntPtr.Zero)
- {
- _parent_scene.remActivePrim(this);
- }
-
- lock (childrenPrim)
- {
- foreach (OdePrim prm in childrenPrim)
- {
- ParentPrim(prm);
- }
- }
-
- }
- private void changeSelectedStatus(float timestep)
- {
- if (m_taintselected)
- {
- m_collisionCategories = CollisionCategories.Selected;
- m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
- // We do the body disable soft twice because 'in theory' a collision could have happened
- // in between the disabling and the collision properties setting
- // which would wake the physical body up from a soft disabling and potentially cause it to fall
- // through the ground.
-
- // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
- // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
- // so that causes the selected part to wake up and continue moving.
- // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
- // assembly will stop simulating during the selection, because of the lack of atomicity
- // of select operations (their processing could be interrupted by a thread switch, causing
- // simulation to continue before all of the selected object notifications trickle down to
- // the physics engine).
- // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
- // selected and disabled. then, due to a thread switch, the selection processing is
- // interrupted and the physics engine continues to simulate, so the last 50 items, whose
- // selection was not yet processed, continues to simulate. this wakes up ALL of the
- // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
- // up, start simulating again, which in turn wakes up the last 50.
- if (m_isphysical)
- {
- disableBodySoft();
- }
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- }
- if (m_isphysical)
- {
- disableBodySoft();
- }
- }
- else
- {
- m_collisionCategories = CollisionCategories.Geom;
- if (m_isphysical)
- m_collisionCategories |= CollisionCategories.Body;
- m_collisionFlags = m_default_collisionFlags;
- if (m_collidesLand)
- m_collisionFlags |= CollisionCategories.Land;
- if (m_collidesWater)
- m_collisionFlags |= CollisionCategories.Water;
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- }
- if (m_isphysical)
- {
- if (Body != IntPtr.Zero)
- {
- d.BodySetLinearVel(Body, 0f, 0f, 0f);
- d.BodySetForce(Body, 0, 0, 0);
- enableBodySoft();
- }
- }
- }
- resetCollisionAccounting();
- m_isSelected = m_taintselected;
- }
- public void ResetTaints()
- {
- m_taintposition = _position;
- m_taintrot = _orientation;
- m_taintPhysics = m_isphysical;
- m_taintselected = m_isSelected;
- m_taintsize = _size;
- m_taintshape = false;
- m_taintforce = false;
- m_taintdisable = false;
- m_taintVelocity = PhysicsVector.Zero;
- }
- public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
- {
- if (_mesh != null)
- {
- setMesh(_parent_scene, _mesh);
- }
- else
- {
- if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
- {
- if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
- {
- if (((_size.X / 2f) > 0f))
- {
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- try
- {
- SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
- }
- catch (AccessViolationException)
- {
- m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
- ode.dunlock(_parent_scene.world);
- return;
- }
- }
- else
- {
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- try
- {
- SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- }
- catch (AccessViolationException)
- {
- m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
- ode.dunlock(_parent_scene.world);
- return;
- }
- }
- }
- else
- {
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- try
- {
- SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- }
- catch (AccessViolationException)
- {
- m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
- ode.dunlock(_parent_scene.world);
- return;
- }
- }
- }
- else
- {
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- try
- {
- SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- }
- catch (AccessViolationException)
- {
- m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
- ode.dunlock(_parent_scene.world);
- return;
- }
- }
- }
- }
- public void changeadd(float timestep)
- {
- int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
- IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
- if (targetspace == IntPtr.Zero)
- targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
- m_targetSpace = targetspace;
- if (_mesh == null)
- {
- if (_parent_scene.needsMeshing(_pbs))
- {
- // Don't need to re-enable body.. it's done in SetMesh
- _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
- // createmesh returns null when it's a shape that isn't a cube.
- }
- }
- lock (_parent_scene.OdeLock)
- {
- CreateGeom(m_targetSpace, _mesh);
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- d.Quaternion myrot = new d.Quaternion();
- myrot.X = _orientation.X;
- myrot.Y = _orientation.Y;
- myrot.Z = _orientation.Z;
- myrot.W = _orientation.W;
- d.GeomSetQuaternion(prim_geom, ref myrot);
- }
- if (m_isphysical && Body == IntPtr.Zero)
- {
- enableBody();
- }
- }
- _parent_scene.geom_name_map[prim_geom] = this.m_primName;
- _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
- changeSelectedStatus(timestep);
- m_taintadd = false;
- }
- public void changemove(float timestep)
- {
- if (m_isphysical)
- {
-
- if (!m_disabled && !m_taintremove && !childPrim)
- {
- if (Body == IntPtr.Zero)
- enableBody();
- //Prim auto disable after 20 frames,
- //if you move it, re-enable the prim manually.
- if (_parent != null)
- {
- if (m_linkJoint != IntPtr.Zero)
- {
- d.JointDestroy(m_linkJoint);
- m_linkJoint = IntPtr.Zero;
- }
- }
- if (Body != IntPtr.Zero)
- {
- d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
- if (_parent != null)
- {
- OdePrim odParent = (OdePrim)_parent;
- if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
- {
- m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
- d.JointAttach(m_linkJoint, Body, odParent.Body);
- d.JointSetFixed(m_linkJoint);
- }
- }
- d.BodyEnable(Body);
- }
- else
- {
- m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
- }
- }
- //else
- // {
- //m_log.Debug("[BUG]: race!");
- //}
- }
- else
- {
- // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
- // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
- m_targetSpace = tempspace;
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- if (prim_geom != IntPtr.Zero)
- {
- d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- d.SpaceAdd(m_targetSpace, prim_geom);
- }
- }
- changeSelectedStatus(timestep);
- resetCollisionAccounting();
- m_taintposition = _position;
- }
- public void Move(float timestep)
- {
- float fx = 0;
- float fy = 0;
- float fz = 0;
- if (IsPhysical && Body != IntPtr.Zero && !m_isSelected)
- {
- //float PID_P = 900.0f;
- float m_mass = CalculateMass();
- fz = 0f;
- //m_log.Info(m_collisionFlags.ToString());
- if (m_buoyancy != 0)
- {
- if (m_buoyancy > 0)
- {
- fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass);
- //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
- //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString());
- }
- else
- {
- fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass));
- }
- }
- if (m_usePID)
- {
- // If we're using the PID controller, then we have no gravity
- fz = (-1 * _parent_scene.gravityz) * m_mass;
- // no lock; for now it's only called from within Simulate()
- // If the PID Controller isn't active then we set our force
- // calculating base velocity to the current position
- if ((m_PIDTau < 1))
- {
- PID_G = PID_G / m_PIDTau;
- }
- if ((PID_G - m_PIDTau) <= 0)
- {
- PID_G = m_PIDTau + 1;
- }
- //PidStatus = true;
- // PhysicsVector vec = new PhysicsVector();
- d.Vector3 vel = d.BodyGetLinearVel(Body);
- d.Vector3 pos = d.BodyGetPosition(Body);
- _target_velocity =
- new PhysicsVector(
- (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
- (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
- (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
- );
- // if velocity is zero, use position control; otherwise, velocity control
- if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f))
- {
- // keep track of where we stopped. No more slippin' & slidin'
- // We only want to deactivate the PID Controller if we think we want to have our surrogate
- // react to the physics scene by moving it's position.
- // Avatar to Avatar collisions
- // Prim to avatar collisions
- //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
- //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
- //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
- d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
- d.BodySetLinearVel(Body, 0, 0, 0);
- d.BodyAddForce(Body, 0, 0, fz);
- return;
- }
- else
- {
- _zeroFlag = false;
- // We're flying and colliding with something
- fx = ((_target_velocity.X) - vel.X) * (PID_D);
- fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
- // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
- fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
- }
- }
- fx *= m_mass;
- fy *= m_mass;
- //fz *= m_mass;
- fx += m_force.X;
- fy += m_force.Y;
- fz += m_force.Z;
- //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
- if (fx != 0 || fy != 0 || fz != 0)
- {
- //m_taintdisable = true;
- //base.RaiseOutOfBounds(Position);
- //d.BodySetLinearVel(Body, fx, fy, 0f);
- if (!d.BodyIsEnabled(Body))
- {
- d.BodySetLinearVel(Body, 0f, 0f, 0f);
- d.BodySetForce(Body, 0, 0, 0);
- enableBodySoft();
- }
- d.BodyAddForce(Body, fx, fy, fz);
- }
- }
- else
- {
- // _zeroPosition = d.BodyGetPosition(Body);
- return;
- }
- }
- public void rotate(float timestep)
- {
- d.Quaternion myrot = new d.Quaternion();
- myrot.X = _orientation.X;
- myrot.Y = _orientation.Y;
- myrot.Z = _orientation.Z;
- myrot.W = _orientation.W;
- d.GeomSetQuaternion(prim_geom, ref myrot);
- if (m_isphysical && Body != IntPtr.Zero)
- {
- d.BodySetQuaternion(Body, ref myrot);
- if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0))
- createAMotor(m_angularlock);
- }
- resetCollisionAccounting();
- m_taintrot = _orientation;
- }
- private void resetCollisionAccounting()
- {
- m_collisionscore = 0;
- m_interpenetrationcount = 0;
- m_disabled = false;
- }
- public void changedisable(float timestep)
- {
- m_disabled = true;
- if (Body != IntPtr.Zero)
- {
- d.BodyDisable(Body);
- Body = IntPtr.Zero;
- }
- m_taintdisable = false;
- }
- public void changePhysicsStatus(float timestep)
- {
- if (m_isphysical == true)
- {
- if (Body == IntPtr.Zero)
- {
- if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
- {
- changeshape(2f);
- }
- else
- {
- enableBody();
- }
- }
- }
- else
- {
- if (Body != IntPtr.Zero)
- {
- if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
- {
-
- if (prim_geom != IntPtr.Zero)
- {
- try
- {
- d.GeomDestroy(prim_geom);
- prim_geom = IntPtr.Zero;
- _mesh = null;
- }
- catch (System.AccessViolationException)
- {
- prim_geom = IntPtr.Zero;
- m_log.Error("[PHYSICS]: PrimGeom dead");
- }
- }
- changeadd(2f);
- }
- if (childPrim)
- {
- if (_parent != null)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildDelink(this);
- }
- }
- else
- {
- disableBody();
- }
- }
- }
- changeSelectedStatus(timestep);
- resetCollisionAccounting();
- m_taintPhysics = m_isphysical;
- }
- public void changesize(float timestamp)
- {
- //if (!_parent_scene.geom_name_map.ContainsKey(prim_geom))
- //{
- // m_taintsize = _size;
- //return;
- //}
- string oldname = _parent_scene.geom_name_map[prim_geom];
- if (_size.X <= 0) _size.X = 0.01f;
- if (_size.Y <= 0) _size.Y = 0.01f;
- if (_size.Z <= 0) _size.Z = 0.01f;
- // Cleanup of old prim geometry
- if (_mesh != null)
- {
- // Cleanup meshing here
- }
- //kill body to rebuild
- if (IsPhysical && Body != IntPtr.Zero)
- {
- if (childPrim)
- {
- if (_parent != null)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildDelink(this);
- }
- }
- else
- {
- disableBody();
- }
- }
- if (d.SpaceQuery(m_targetSpace, prim_geom))
- {
- _parent_scene.waitForSpaceUnlock(m_targetSpace);
- d.SpaceRemove(m_targetSpace, prim_geom);
- }
- d.GeomDestroy(prim_geom);
- prim_geom = IntPtr.Zero;
- // we don't need to do space calculation because the client sends a position update also.
- // Construction of new prim
- if (_parent_scene.needsMeshing(_pbs))
- {
- float meshlod = _parent_scene.meshSculptLOD;
- if (IsPhysical)
- meshlod = _parent_scene.MeshSculptphysicalLOD;
- // Don't need to re-enable body.. it's done in SetMesh
- IMesh mesh = null;
- if (_parent_scene.needsMeshing(_pbs))
- mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
- //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
- CreateGeom(m_targetSpace, mesh);
-
- }
- else
- {
- _mesh = null;
- CreateGeom(m_targetSpace, _mesh);
- }
- d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- d.Quaternion myrot = new d.Quaternion();
- myrot.X = _orientation.X;
- myrot.Y = _orientation.Y;
- myrot.Z = _orientation.Z;
- myrot.W = _orientation.W;
- d.GeomSetQuaternion(prim_geom, ref myrot);
- //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
- if (IsPhysical && Body == IntPtr.Zero && !childPrim)
- {
- // Re creates body on size.
- // EnableBody also does setMass()
- enableBody();
- d.BodyEnable(Body);
- }
- _parent_scene.geom_name_map[prim_geom] = oldname;
- changeSelectedStatus(timestamp);
- if (childPrim)
- {
- if (_parent is OdePrim)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildSetGeom(this);
- }
- }
- resetCollisionAccounting();
- m_taintsize = _size;
- }
- //public void changesize(float timestamp)
- //{
- // //if (!_parent_scene.geom_name_map.ContainsKey(prim_geom))
- // //{
- // // m_taintsize = _size;
- // //return;
- // //}
- // string oldname = _parent_scene.geom_name_map[prim_geom];
- // if (_size.X <= 0) _size.X = 0.01f;
- // if (_size.Y <= 0) _size.Y = 0.01f;
- // if (_size.Z <= 0) _size.Z = 0.01f;
- // // Cleanup of old prim geometry
- // if (_mesh != null)
- // {
- // // Cleanup meshing here
- // }
- // //kill body to rebuild
- // if (IsPhysical && Body != (IntPtr) 0)
- // {
- // disableBody();
- // }
- // if (d.SpaceQuery(m_targetSpace, prim_geom))
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // d.SpaceRemove(m_targetSpace, prim_geom);
- // }
- // d.GeomDestroy(prim_geom);
- // prim_geom = (IntPtr)0;
- // // we don't need to do space calculation because the client sends a position update also.
- // // Construction of new prim
- // if (_parent_scene.needsMeshing(_pbs))
- // {
- // float meshlod = _parent_scene.meshSculptLOD;
- // if (IsPhysical)
- // meshlod = _parent_scene.MeshSculptphysicalLOD;
- // // Don't need to re-enable body.. it's done in SetMesh
- // IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
- // // createmesh returns null when it's a shape that isn't a cube.
- // if (mesh != null)
- // {
- // setMesh(_parent_scene, mesh);
- // d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- // d.Quaternion myrot = new d.Quaternion();
- // myrot.W = _orientation.w;
- // myrot.X = _orientation.X;
- // myrot.Y = _orientation.Y;
- // myrot.Z = _orientation.Z;
- // d.GeomSetQuaternion(prim_geom, ref myrot);
- // //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
- // if (IsPhysical && Body == (IntPtr)0)
- // {
- // // Re creates body on size.
- // // EnableBody also does setMass()
- // enableBody();
- // d.BodyEnable(Body);
- // }
- // }
- // else
- // {
- // if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
- // {
- // if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
- // {
- // if (((_size.X / 2f) > 0f) && ((_size.X / 2f) < 1000))
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
- // }
- // else
- // {
- // m_log.Info("[PHYSICS]: Failed to load a sphere bad size");
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- // }
- // }
- // else
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- // }
- // }
- // //else if (_pbs.ProfileShape == ProfileShape.Circle && _pbs.PathCurve == (byte)Extrusion.Straight)
- // //{
- // //Cyllinder
- // //if (_size.X == _size.Y)
- // //{
- // // prim_geom = d.CreateCylinder(m_targetSpace, _size.X / 2, _size.Z);
- // //}
- // //else
- // //{
- // //prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
- // //}
- // //}
- // else
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- // }
- // //prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
- // d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- // d.Quaternion myrot = new d.Quaternion();
- // myrot.W = _orientation.w;
- // myrot.X = _orientation.X;
- // myrot.Y = _orientation.Y;
- // myrot.Z = _orientation.Z;
- // d.GeomSetQuaternion(prim_geom, ref myrot);
- // }
- // }
- // else
- // {
- // if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
- // {
- // if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
- // }
- // else
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- // }
- // }
- // //else if (_pbs.ProfileShape == ProfileShape.Circle && _pbs.PathCurve == (byte)Extrusion.Straight)
- // //{
- // //Cyllinder
- // //if (_size.X == _size.Y)
- // //{
- // //prim_geom = d.CreateCylinder(m_targetSpace, _size.X / 2, _size.Z);
- // //}
- // //else
- // //{
- // //prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
- // //}
- // //}
- // else
- // {
- // _parent_scene.waitForSpaceUnlock(m_targetSpace);
- // SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
- // }
- // d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- // d.Quaternion myrot = new d.Quaternion();
- // myrot.W = _orientation.w;
- // myrot.X = _orientation.X;
- // myrot.Y = _orientation.Y;
- // myrot.Z = _orientation.Z;
- // d.GeomSetQuaternion(prim_geom, ref myrot);
- // //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
- // if (IsPhysical && Body == (IntPtr) 0)
- // {
- // // Re creates body on size.
- // // EnableBody also does setMass()
- // enableBody();
- // d.BodyEnable(Body);
- // }
- // }
- // _parent_scene.geom_name_map[prim_geom] = oldname;
- // changeSelectedStatus(timestamp);
- // resetCollisionAccounting();
- // m_taintsize = _size;
- //}
- public void changefloatonwater(float timestep)
- {
- m_collidesWater = m_taintCollidesWater;
- if (prim_geom != IntPtr.Zero)
- {
- if (m_collidesWater)
- {
- m_collisionFlags |= CollisionCategories.Water;
- }
- else
- {
- m_collisionFlags &= ~CollisionCategories.Water;
- }
- d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
- }
- }
- public void changeshape(float timestamp)
- {
- string oldname = _parent_scene.geom_name_map[prim_geom];
- // Cleanup of old prim geometry and Bodies
- if (IsPhysical && Body != IntPtr.Zero)
- {
- if (childPrim)
- {
- if (_parent != null)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildDelink(this);
- }
- }
- else
- {
- disableBody();
- }
- }
- try
- {
- d.GeomDestroy(prim_geom);
- }
- catch (System.AccessViolationException)
- {
- prim_geom = IntPtr.Zero;
- m_log.Error("[PHYSICS]: PrimGeom dead");
- }
- prim_geom = IntPtr.Zero;
- // we don't need to do space calculation because the client sends a position update also.
- if (_size.X <= 0) _size.X = 0.01f;
- if (_size.Y <= 0) _size.Y = 0.01f;
- if (_size.Z <= 0) _size.Z = 0.01f;
- // Construction of new prim
- if (_parent_scene.needsMeshing(_pbs))
- {
- // Don't need to re-enable body.. it's done in SetMesh
- float meshlod = _parent_scene.meshSculptLOD;
- if (IsPhysical)
- meshlod = _parent_scene.MeshSculptphysicalLOD;
- IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
- // createmesh returns null when it doesn't mesh.
- CreateGeom(m_targetSpace, mesh);
- }
- else
- {
- _mesh = null;
- CreateGeom(m_targetSpace, null);
- }
- d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- d.Quaternion myrot = new d.Quaternion();
- //myrot.W = _orientation.w;
- myrot.W = _orientation.W;
- myrot.X = _orientation.X;
- myrot.Y = _orientation.Y;
- myrot.Z = _orientation.Z;
- d.GeomSetQuaternion(prim_geom, ref myrot);
- //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
- if (IsPhysical && Body == IntPtr.Zero)
- {
- // Re creates body on size.
- // EnableBody also does setMass()
- enableBody();
- d.BodyEnable(Body);
- }
- _parent_scene.geom_name_map[prim_geom] = oldname;
- changeSelectedStatus(timestamp);
- if (childPrim)
- {
- if (_parent is OdePrim)
- {
- OdePrim parent = (OdePrim)_parent;
- parent.ChildSetGeom(this);
- }
- }
- resetCollisionAccounting();
- m_taintshape = false;
- }
- public void changeAddForce(float timestamp)
- {
- if (!m_isSelected)
- {
- lock (m_forcelist)
- {
- //m_log.Info("[PHYSICS]: dequeing forcelist");
- if (IsPhysical)
- {
- PhysicsVector iforce = new PhysicsVector();
- for (int i = 0; i < m_forcelist.Count; i++)
- {
- iforce = iforce + (m_forcelist[i] * 100);
- }
- d.BodyEnable(Body);
- d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
- }
- m_forcelist.Clear();
- }
- m_collisionscore = 0;
- m_interpenetrationcount = 0;
- }
- m_taintforce = false;
- }
- public void changeSetTorque(float timestamp)
- {
- if (!m_isSelected)
- {
- if (IsPhysical && Body != IntPtr.Zero)
- {
- d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
- }
- }
-
- m_taintTorque = new PhysicsVector(0, 0, 0);
- }
- public void changeAddAngularForce(float timestamp)
- {
- if (!m_isSelected)
- {
- lock (m_angularforcelist)
- {
- //m_log.Info("[PHYSICS]: dequeing forcelist");
- if (IsPhysical)
- {
- PhysicsVector iforce = new PhysicsVector();
- for (int i = 0; i < m_angularforcelist.Count; i++)
- {
- iforce = iforce + (m_angularforcelist[i] * 100);
- }
- d.BodyEnable(Body);
- d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
-
- }
- m_angularforcelist.Clear();
- }
- m_collisionscore = 0;
- m_interpenetrationcount = 0;
- }
- m_taintaddangularforce = false;
- }
- private void changevelocity(float timestep)
- {
- if (!m_isSelected)
- {
- Thread.Sleep(20);
- if (IsPhysical)
- {
- if (Body != IntPtr.Zero)
- {
- d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
- }
- }
-
- //resetCollisionAccounting();
- }
- m_taintVelocity = PhysicsVector.Zero;
- }
- public override bool IsPhysical
- {
- get { return m_isphysical; }
- set { m_isphysical = value; }
- }
- public void setPrimForRemoval()
- {
- m_taintremove = true;
- }
- public override bool Flying
- {
- // no flying prims for you
- get { return false; }
- set { }
- }
- public override bool IsColliding
- {
- get { return iscolliding; }
- set { iscolliding = value; }
- }
- public override bool CollidingGround
- {
- get { return false; }
- set { return; }
- }
- public override bool CollidingObj
- {
- get { return false; }
- set { return; }
- }
- public override bool ThrottleUpdates
- {
- get { return m_throttleUpdates; }
- set { m_throttleUpdates = value; }
- }
- public override bool Stopped
- {
- get { return _zeroFlag; }
- }
- public override PhysicsVector Position
- {
- get { return _position; }
- set { _position = value;
- //m_log.Info("[PHYSICS]: " + _position.ToString());
- }
- }
- public override PhysicsVector Size
- {
- get { return _size; }
- set { _size = value; }
- }
- public override float Mass
- {
- get { return CalculateMass(); }
- }
- public override PhysicsVector Force
- {
- //get { return PhysicsVector.Zero; }
- get { return m_force; }
- set { m_force = value; }
- }
- public override int VehicleType
- {
- get { return 0; }
- set { return; }
- }
- public override void VehicleFloatParam(int param, float value)
- {
- }
- public override void VehicleVectorParam(int param, PhysicsVector value)
- {
- }
- public override void VehicleRotationParam(int param, Quaternion rotation)
- {
- }
- public override void SetVolumeDetect(int param)
- {
- lock (_parent_scene.OdeLock)
- {
- m_isVolumeDetect = (param!=0);
- }
- }
- public override PhysicsVector CenterOfMass
- {
- get { return PhysicsVector.Zero; }
- }
- public override PhysicsVector GeometricCenter
- {
- get { return PhysicsVector.Zero; }
- }
- public override PrimitiveBaseShape Shape
- {
- set
- {
- _pbs = value;
- m_taintshape = true;
- }
- }
- public override PhysicsVector Velocity
- {
- get
- {
- // Averate previous velocity with the new one so
- // client object interpolation works a 'little' better
- PhysicsVector returnVelocity = new PhysicsVector();
- returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
- returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
- returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
- return returnVelocity;
- }
- set
- {
- _velocity = value;
- m_taintVelocity = value;
- _parent_scene.AddPhysicsActorTaint(this);
- }
- }
- public override PhysicsVector Torque
- {
- get
- {
- if (!m_isphysical || Body == IntPtr.Zero)
- return new PhysicsVector(0,0,0);
- return _torque;
- }
- set
- {
- m_taintTorque = value;
- _parent_scene.AddPhysicsActorTaint(this);
- }
- }
- public override float CollisionScore
- {
- get { return m_collisionscore; }
- set { m_collisionscore = value; }
- }
- public override bool Kinematic
- {
- get { return false; }
- set { }
- }
- public override Quaternion Orientation
- {
- get { return _orientation; }
- set { _orientation = value; }
- }
- public override PhysicsVector Acceleration
- {
- get { return _acceleration; }
- }
- public void SetAcceleration(PhysicsVector accel)
- {
- _acceleration = accel;
- }
- public override void AddForce(PhysicsVector force, bool pushforce)
- {
- m_forcelist.Add(force);
- m_taintforce = true;
- //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
- }
- public override void AddAngularForce(PhysicsVector force, bool pushforce)
- {
- m_angularforcelist.Add(force);
- m_taintaddangularforce = true;
- }
- public override PhysicsVector RotationalVelocity
- {
- get
- {
- PhysicsVector pv = new PhysicsVector(0, 0, 0);
- if (_zeroFlag)
- return pv;
- m_lastUpdateSent = false;
- if (m_rotationalVelocity.IsIdentical(pv, 0.2f))
- return pv;
- return m_rotationalVelocity;
- }
- set { m_rotationalVelocity = value; }
- }
- public override void CrossingFailure()
- {
- m_crossingfailures++;
- if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
- {
- base.RaiseOutOfBounds(_position);
- return;
- }
- else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
- {
- m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
- }
- }
- public override float Buoyancy
- {
- get { return m_buoyancy; }
- set { m_buoyancy = value; }
- }
- public override void link(PhysicsActor obj)
- {
- m_taintparent = obj;
- }
- public override void delink()
- {
- m_taintparent = null;
- }
- public override void LockAngularMotion(PhysicsVector axis)
- {
- // reverse the zero/non zero values for ODE.
- axis.X = (axis.X > 0) ? 1f : 0f;
- axis.Y = (axis.Y > 0) ? 1f : 0f;
- axis.Z = (axis.Z > 0) ? 1f : 0f;
- m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
- m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z); ;
- }
- public void UpdatePositionAndVelocity()
- {
- // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
- if (_parent == null)
- {
- PhysicsVector pv = new PhysicsVector(0, 0, 0);
- bool lastZeroFlag = _zeroFlag;
- if (Body != (IntPtr)0) // FIXME -> or if it is a joint
- {
- d.Vector3 vec = d.BodyGetPosition(Body);
- d.Quaternion ori = d.BodyGetQuaternion(Body);
- d.Vector3 vel = d.BodyGetLinearVel(Body);
- d.Vector3 rotvel = d.BodyGetAngularVel(Body);
- d.Vector3 torque = d.BodyGetTorque(Body);
- _torque.setValues(torque.X, torque.Y, torque.Z);
- PhysicsVector l_position = new PhysicsVector();
- Quaternion l_orientation = new Quaternion();
- // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
- //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
- //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
- //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
- //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
- m_lastposition = _position;
- m_lastorientation = _orientation;
- l_position.X = vec.X;
- l_position.Y = vec.Y;
- l_position.Z = vec.Z;
- l_orientation.X = ori.X;
- l_orientation.Y = ori.Y;
- l_orientation.Z = ori.Z;
- l_orientation.W = ori.W;
- if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f)
- {
- //base.RaiseOutOfBounds(l_position);
- if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
- {
- _position = l_position;
- //_parent_scene.remActivePrim(this);
- if (_parent == null)
- base.RequestPhysicsterseUpdate();
- return;
- }
- else
- {
- if (_parent == null)
- base.RaiseOutOfBounds(l_position);
- return;
- }
- }
- if (l_position.Z < 0)
- {
- // This is so prim that get lost underground don't fall forever and suck up
- //
- // Sim resources and memory.
- // Disables the prim's movement physics....
- // It's a hack and will generate a console message if it fails.
- //IsPhysical = false;
- if (_parent == null)
- base.RaiseOutOfBounds(_position);
- _acceleration.X = 0;
- _acceleration.Y = 0;
- _acceleration.Z = 0;
- _velocity.X = 0;
- _velocity.Y = 0;
- _velocity.Z = 0;
- m_rotationalVelocity.X = 0;
- m_rotationalVelocity.Y = 0;
- m_rotationalVelocity.Z = 0;
- if (_parent == null)
- base.RequestPhysicsterseUpdate();
- m_throttleUpdates = false;
- throttleCounter = 0;
- _zeroFlag = true;
- //outofBounds = true;
- }
- if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
- && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
- && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
- && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01 ))
- {
- _zeroFlag = true;
- m_throttleUpdates = false;
- }
- else
- {
- //System.Console.WriteLine(Math.Abs(m_lastposition.X - l_position.X).ToString());
- _zeroFlag = false;
- }
- if (_zeroFlag)
- {
- _velocity.X = 0.0f;
- _velocity.Y = 0.0f;
- _velocity.Z = 0.0f;
- _acceleration.X = 0;
- _acceleration.Y = 0;
- _acceleration.Z = 0;
- //_orientation.w = 0f;
- //_orientation.X = 0f;
- //_orientation.Y = 0f;
- //_orientation.Z = 0f;
- m_rotationalVelocity.X = 0;
- m_rotationalVelocity.Y = 0;
- m_rotationalVelocity.Z = 0;
- if (!m_lastUpdateSent)
- {
- m_throttleUpdates = false;
- throttleCounter = 0;
- m_rotationalVelocity = pv;
- if (_parent == null)
- base.RequestPhysicsterseUpdate();
- m_lastUpdateSent = true;
- }
- }
- else
- {
- if (lastZeroFlag != _zeroFlag)
- {
- if (_parent == null)
- base.RequestPhysicsterseUpdate();
- }
- m_lastVelocity = _velocity;
- _position = l_position;
- _velocity.X = vel.X;
- _velocity.Y = vel.Y;
- _velocity.Z = vel.Z;
- _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
- _acceleration = new PhysicsVector(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
- //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
- if (_velocity.IsIdentical(pv, 0.5f))
- {
- m_rotationalVelocity = pv;
- }
- else
- {
- m_rotationalVelocity.setValues(rotvel.X, rotvel.Y, rotvel.Z);
- }
- //System.Console.WriteLine("ODE: " + m_rotationalVelocity.ToString());
- _orientation.X = ori.X;
- _orientation.Y = ori.Y;
- _orientation.Z = ori.Z;
- _orientation.W = ori.W;
- m_lastUpdateSent = false;
- if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
- {
- if (_parent == null)
- base.RequestPhysicsterseUpdate();
- }
- else
- {
- throttleCounter++;
- }
- }
- m_lastposition = l_position;
- }
- else
- {
- // Not a body.. so Make sure the client isn't interpolating
- _velocity.X = 0;
- _velocity.Y = 0;
- _velocity.Z = 0;
- _acceleration.X = 0;
- _acceleration.Y = 0;
- _acceleration.Z = 0;
- m_rotationalVelocity.X = 0;
- m_rotationalVelocity.Y = 0;
- m_rotationalVelocity.Z = 0;
- _zeroFlag = true;
- }
- }
- }
- public override bool FloatOnWater
- {
- set {
- m_taintCollidesWater = value;
- _parent_scene.AddPhysicsActorTaint(this);
- }
- }
- public override void SetMomentum(PhysicsVector momentum)
- {
- }
- public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }
- public override bool PIDActive { set { m_usePID = value; } }
- public override float PIDTau { set { m_PIDTau = value; } }
- private void createAMotor(PhysicsVector 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));
-
- if (axisnum <= 0)
- return;
- int dAMotorEuler = 1;
- Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
- d.JointAttach(Amotor, Body, IntPtr.Zero);
- 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);
- }
- //d.JointSetAMotorAngle(Amotor, 1, 0);
- //d.JointSetAMotorAngle(Amotor, 2, 0);
- // These lowstops and high stops are effectively (no wiggle room)
- d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
- d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
- d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
- d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
- d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
- d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
- d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
- d.JointSetAMotorParam(Amotor, (int)dParam.FMax, m_tensor * 5);//
- }
- public override void SubscribeEvents(int ms)
- {
- m_eventsubscription = ms;
- _parent_scene.addCollisionEventReporting(this);
- }
- public override void UnSubscribeEvents()
- {
- _parent_scene.remCollisionEventReporting(this);
- m_eventsubscription = 0;
- }
- public void AddCollisionEvent(uint CollidedWith, float depth)
- {
- if (CollisionEventsThisFrame == null)
- CollisionEventsThisFrame = new CollisionEventUpdate();
- CollisionEventsThisFrame.addCollider(CollidedWith,depth);
- }
- public void SendCollisions()
- {
- if (CollisionEventsThisFrame == null)
- return;
- base.SendCollisionUpdate(CollisionEventsThisFrame);
- if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
- CollisionEventsThisFrame = null;
- else
- CollisionEventsThisFrame = new CollisionEventUpdate();
- }
- public override bool SubscribedEvents()
- {
- if (m_eventsubscription > 0)
- return true;
- return false;
- }
- }
- }
|