Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

opensim-development – Blame information for rev 1

Rev	Author	Line No.	Line
1	eva	1	/*
		2	* Copyright (c) Contributors, http://opensimulator.org/
		3	* See CONTRIBUTORS.TXT for a full list of copyright holders.
		4	*
		5	* Redistribution and use in source and binary forms, with or without
		6	* modification, are permitted provided that the following conditions are met:
		7	* * Redistributions of source code must retain the above copyright
		8	* notice, this list of conditions and the following disclaimer.
		9	* * Redistributions in binary form must reproduce the above copyright
		10	* notice, this list of conditions and the following disclaimer in the
		11	* documentation and/or other materials provided with the distribution.
		12	* * Neither the name of the OpenSimulator Project nor the
		13	* names of its contributors may be used to endorse or promote products
		14	* derived from this software without specific prior written permission.
		15	*
		16	* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
		17	* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
		18	* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
		19	* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
		20	* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
		21	* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
		22	* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
		23	* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
		24	* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
		25	* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
		26	*/
		27
		28	/*
		29	* Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
		30	* ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
		31	* ODEPrim.cs contains methods dealing with Prim editing, Prim
		32	* characteristics and Kinetic motion.
		33	* ODEDynamics.cs contains methods dealing with Prim Physical motion
		34	* (dynamics) and the associated settings. Old Linear and angular
		35	* motors for dynamic motion have been replace with MoveLinear()
		36	* and MoveAngular(); 'Physical' is used only to switch ODE dynamic
		37	* simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
		38	* switch between 'VEHICLE' parameter use and general dynamics
		39	* settings use.
		40	*/
		41
		42	//#define SPAM
		43
		44	using System;
		45	using System.Collections.Generic;
		46	using System.Reflection;
		47	using System.Runtime.InteropServices;
		48	using System.Threading;
		49	using log4net;
		50	using OpenMetaverse;
		51	using Ode.NET;
		52	using OpenSim.Framework;
		53	using OpenSim.Region.Physics.Manager;
		54
		55	namespace OpenSim.Region.Physics.OdePlugin
		56	{
		57	/// <summary>
		58	/// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
		59	/// </summary>
		60	public class OdePrim : PhysicsActor
		61	{
		62	private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
		63
		64	private bool m_isphysical;
		65
		66	public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
		67	private int m_expectedCollisionContacts = 0;
		68
		69	/// <summary>
		70	/// Gets collide bits so that we can still perform land collisions if a mesh fails to load.
		71	/// </summary>
		72	private int BadMeshAssetCollideBits
		73	{
		74	get { return m_isphysical ? (int)CollisionCategories.Land : 0; }
		75	}
		76
		77	/// <summary>
		78	/// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
		79	/// </summary>
		80	public override bool IsPhysical
		81	{
		82	get { return m_isphysical; }
		83	set
		84	{
		85	m_isphysical = value;
		86	if (!m_isphysical) // Zero the remembered last velocity
		87	m_lastVelocity = Vector3.Zero;
		88	}
		89	}
		90
		91	private Vector3 _position;
		92	private Vector3 _velocity;
		93	private Vector3 _torque;
		94	private Vector3 m_lastVelocity;
		95	private Vector3 m_lastposition;
		96	private Quaternion m_lastorientation = new Quaternion();
		97	private Vector3 m_rotationalVelocity;
		98	private Vector3 _size;
		99	private Vector3 _acceleration;
		100	// private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
		101	private Quaternion _orientation;
		102	private Vector3 m_taintposition;
		103	private Vector3 m_taintsize;
		104	private Vector3 m_taintVelocity;
		105	private Vector3 m_taintTorque;
		106	private Quaternion m_taintrot;
		107	private Vector3 m_angularlock = Vector3.One;
		108	private Vector3 m_taintAngularLock = Vector3.One;
		109	private IntPtr Amotor = IntPtr.Zero;
		110
		111	private bool m_assetFailed = false;
		112
		113	private Vector3 m_PIDTarget;
		114	private float m_PIDTau;
		115	private float PID_D = 35f;
		116	private float PID_G = 25f;
		117	private bool m_usePID;
		118
		119	// KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
		120	// and are for non-VEHICLES only.
		121
		122	private float m_PIDHoverHeight;
		123	private float m_PIDHoverTau;
		124	private bool m_useHoverPID;
		125	private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
		126	private float m_targetHoverHeight;
		127	private float m_groundHeight;
		128	private float m_waterHeight;
		129	private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
		130
		131	// private float m_tensor = 5f;
		132	private int body_autodisable_frames = 20;
		133
		134
		135	private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
		136	| CollisionCategories.Space
		137	| CollisionCategories.Body
		138	| CollisionCategories.Character
		139	);
		140	private bool m_taintshape;
		141	private bool m_taintPhysics;
		142	private bool m_collidesLand = true;
		143	private bool m_collidesWater;
		144
		145	// Default we're a Geometry
		146	private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
		147
		148	// Default, Collide with Other Geometries, spaces and Bodies
		149	private CollisionCategories m_collisionFlags = m_default_collisionFlags;
		150
		151	public bool m_taintremove { get; private set; }
		152	public bool m_taintdisable { get; private set; }
		153	internal bool m_disabled;
		154	public bool m_taintadd { get; private set; }
		155	public bool m_taintselected { get; private set; }
		156	public bool m_taintCollidesWater { get; private set; }
		157
		158	private bool m_taintforce = false;
		159	private bool m_taintaddangularforce = false;
		160	private Vector3 m_force;
		161	private List<Vector3> m_forcelist = new List<Vector3>();
		162	private List<Vector3> m_angularforcelist = new List<Vector3>();
		163
		164	private PrimitiveBaseShape _pbs;
		165	private OdeScene _parent_scene;
		166
		167	/// <summary>
		168	/// The physics space which contains prim geometries
		169	/// </summary>
		170	public IntPtr m_targetSpace = IntPtr.Zero;
		171
		172	/// <summary>
		173	/// The prim geometry, used for collision detection.
		174	/// </summary>
		175	/// <remarks>
		176	/// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
		177	/// mesh change) or when the physical prim is being removed from the scene.
		178	/// </remarks>
		179	public IntPtr prim_geom { get; private set; }
		180
		181	public IntPtr _triMeshData { get; private set; }
		182
		183	private IntPtr _linkJointGroup = IntPtr.Zero;
		184	private PhysicsActor _parent;
		185	private PhysicsActor m_taintparent;
		186
		187	private List<OdePrim> childrenPrim = new List<OdePrim>();
		188
		189	private bool iscolliding;
		190	private bool m_isSelected;
		191
		192	internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
		193
		194	private bool m_throttleUpdates;
		195	private int throttleCounter;
		196	public int m_interpenetrationcount { get; private set; }
		197	internal float m_collisionscore;
		198	public int m_roundsUnderMotionThreshold { get; private set; }
		199	private int m_crossingfailures;
		200
		201	public bool outofBounds { get; private set; }
		202	private float m_density = 10.000006836f; // Aluminum g/cm3;
		203
		204	public bool _zeroFlag { get; private set; }
		205	private bool m_lastUpdateSent;
		206
		207	public IntPtr Body = IntPtr.Zero;
		208	private Vector3 _target_velocity;
		209	private d.Mass pMass;
		210
		211	private int m_eventsubscription;
		212	private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
		213
		214	/// <summary>
		215	/// Signal whether there were collisions on the previous frame, so we know if we need to send the
		216	/// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
		217	/// </summary>
		218	/// <remarks>
		219	/// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
		220	/// </remarks>
		221	private bool m_collisionsOnPreviousFrame;
		222
		223	private IntPtr m_linkJoint = IntPtr.Zero;
		224
		225	internal volatile bool childPrim;
		226
		227	private ODEDynamics m_vehicle;
		228
		229	internal int m_material = (int)Material.Wood;
		230
		231	public OdePrim(
		232	String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
		233	Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
		234	{
		235	Name = primName;
		236	m_vehicle = new ODEDynamics();
		237	//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
		238
		239	if (!pos.IsFinite())
		240	{
		241	pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
		242	parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
		243	m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
		244	}
		245	_position = pos;
		246	m_taintposition = pos;
		247	PID_D = parent_scene.bodyPIDD;
		248	PID_G = parent_scene.bodyPIDG;
		249	m_density = parent_scene.geomDefaultDensity;
		250	// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
		251	body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
		252
		253	prim_geom = IntPtr.Zero;
		254
		255	if (!pos.IsFinite())
		256	{
		257	size = new Vector3(0.5f, 0.5f, 0.5f);
		258	m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
		259	}
		260
		261	if (size.X <= 0) size.X = 0.01f;
		262	if (size.Y <= 0) size.Y = 0.01f;
		263	if (size.Z <= 0) size.Z = 0.01f;
		264
		265	_size = size;
		266	m_taintsize = _size;
		267
		268	if (!QuaternionIsFinite(rotation))
		269	{
		270	rotation = Quaternion.Identity;
		271	m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
		272	}
		273
		274	_orientation = rotation;
		275	m_taintrot = _orientation;
		276	_pbs = pbs;
		277
		278	_parent_scene = parent_scene;
		279	m_targetSpace = (IntPtr)0;
		280
		281	if (pos.Z < 0)
		282	{
		283	IsPhysical = false;
		284	}
		285	else
		286	{
		287	IsPhysical = pisPhysical;
		288	// If we're physical, we need to be in the master space for now.
		289	// linksets *should* be in a space together.. but are not currently
		290	if (IsPhysical)
		291	m_targetSpace = _parent_scene.space;
		292	}
		293
		294	m_taintadd = true;
		295	m_assetFailed = false;
		296	_parent_scene.AddPhysicsActorTaint(this);
		297	}
		298
		299	public override int PhysicsActorType
		300	{
		301	get { return (int) ActorTypes.Prim; }
		302	set { return; }
		303	}
		304
		305	public override bool SetAlwaysRun
		306	{
		307	get { return false; }
		308	set { return; }
		309	}
		310
		311	public override bool Grabbed
		312	{
		313	set { return; }
		314	}
		315
		316	public override bool Selected
		317	{
		318	set
		319	{
		320	// This only makes the object not collidable if the object
		321	// is physical or the object is modified somehow *IN THE FUTURE*
		322	// without this, if an avatar selects prim, they can walk right
		323	// through it while it's selected
		324	m_collisionscore = 0;
		325
		326	if ((IsPhysical && !_zeroFlag) || !value)
		327	{
		328	m_taintselected = value;
		329	_parent_scene.AddPhysicsActorTaint(this);
		330	}
		331	else
		332	{
		333	m_taintselected = value;
		334	m_isSelected = value;
		335	}
		336
		337	if (m_isSelected)
		338	disableBodySoft();
		339	}
		340	}
		341
		342	/// <summary>
		343	/// Set a new geometry for this prim.
		344	/// </summary>
		345	/// <param name="geom"></param>
		346	private void SetGeom(IntPtr geom)
		347	{
		348	prim_geom = geom;
		349	//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
		350
		351	if (m_assetFailed)
		352	{
		353	d.GeomSetCategoryBits(prim_geom, 0);
		354	d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
		355	}
		356	else
		357	{
		358	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		359	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		360	}
		361
		362	_parent_scene.geom_name_map[prim_geom] = Name;
		363	_parent_scene.actor_name_map[prim_geom] = this;
		364
		365	if (childPrim)
		366	{
		367	if (_parent != null && _parent is OdePrim)
		368	{
		369	OdePrim parent = (OdePrim)_parent;
		370	//Console.WriteLine("SetGeom calls ChildSetGeom");
		371	parent.ChildSetGeom(this);
		372	}
		373	}
		374	//m_log.Warn("Setting Geom to: " + prim_geom);
		375	}
		376
		377	private void enableBodySoft()
		378	{
		379	if (!childPrim)
		380	{
		381	if (IsPhysical && Body != IntPtr.Zero)
		382	{
		383	d.BodyEnable(Body);
		384	if (m_vehicle.Type != Vehicle.TYPE_NONE)
		385	m_vehicle.Enable(Body, _parent_scene);
		386	}
		387
		388	m_disabled = false;
		389	}
		390	}
		391
		392	private void disableBodySoft()
		393	{
		394	m_disabled = true;
		395
		396	if (IsPhysical && Body != IntPtr.Zero)
		397	{
		398	d.BodyDisable(Body);
		399	}
		400	}
		401
		402	/// <summary>
		403	/// Make a prim subject to physics.
		404	/// </summary>
		405	private void enableBody()
		406	{
		407	// Don't enable this body if we're a child prim
		408	// this should be taken care of in the parent function not here
		409	if (!childPrim)
		410	{
		411	// Sets the geom to a body
		412	Body = d.BodyCreate(_parent_scene.world);
		413
		414	setMass();
		415	d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
		416	d.Quaternion myrot = new d.Quaternion();
		417	myrot.X = _orientation.X;
		418	myrot.Y = _orientation.Y;
		419	myrot.Z = _orientation.Z;
		420	myrot.W = _orientation.W;
		421	d.BodySetQuaternion(Body, ref myrot);
		422	d.GeomSetBody(prim_geom, Body);
		423
		424	if (m_assetFailed)
		425	{
		426	d.GeomSetCategoryBits(prim_geom, 0);
		427	d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
		428	}
		429	else
		430	{
		431	m_collisionCategories |= CollisionCategories.Body;
		432	m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
		433	}
		434
		435	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		436	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		437
		438	d.BodySetAutoDisableFlag(Body, true);
		439	d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
		440
		441	// disconnect from world gravity so we can apply buoyancy
		442	d.BodySetGravityMode (Body, false);
		443
		444	m_interpenetrationcount = 0;
		445	m_collisionscore = 0;
		446	m_disabled = false;
		447
		448	// The body doesn't already have a finite rotation mode set here
		449	if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
		450	{
		451	createAMotor(m_angularlock);
		452	}
		453	if (m_vehicle.Type != Vehicle.TYPE_NONE)
		454	{
		455	m_vehicle.Enable(Body, _parent_scene);
		456	}
		457
		458	_parent_scene.ActivatePrim(this);
		459	}
		460	}
		461
		462	#region Mass Calculation
		463
		464	private float CalculateMass()
		465	{
		466	float volume = _size.X * _size.Y * _size.Z; // default
		467	float tmp;
		468
		469	float returnMass = 0;
		470	float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
		471	float hollowVolume = hollowAmount * hollowAmount;
		472
		473	switch (_pbs.ProfileShape)
		474	{
		475	case ProfileShape.Square:
		476	// default box
		477
		478	if (_pbs.PathCurve == (byte)Extrusion.Straight)
		479	{
		480	if (hollowAmount > 0.0)
		481	{
		482	switch (_pbs.HollowShape)
		483	{
		484	case HollowShape.Square:
		485	case HollowShape.Same:
		486	break;
		487
		488	case HollowShape.Circle:
		489
		490	hollowVolume *= 0.78539816339f;
		491	break;
		492
		493	case HollowShape.Triangle:
		494
		495	hollowVolume *= (0.5f * .5f);
		496	break;
		497
		498	default:
		499	hollowVolume = 0;
		500	break;
		501	}
		502	volume *= (1.0f - hollowVolume);
		503	}
		504	}
		505
		506	else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
		507	{
		508	//a tube
		509
		510	volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
		511	tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
		512	volume -= volume*tmp*tmp;
		513
		514	if (hollowAmount > 0.0)
		515	{
		516	hollowVolume *= hollowAmount;
		517
		518	switch (_pbs.HollowShape)
		519	{
		520	case HollowShape.Square:
		521	case HollowShape.Same:
		522	break;
		523
		524	case HollowShape.Circle:
		525	hollowVolume *= 0.78539816339f;;
		526	break;
		527
		528	case HollowShape.Triangle:
		529	hollowVolume *= 0.5f * 0.5f;
		530	break;
		531	default:
		532	hollowVolume = 0;
		533	break;
		534	}
		535	volume *= (1.0f - hollowVolume);
		536	}
		537	}
		538
		539	break;
		540
		541	case ProfileShape.Circle:
		542
		543	if (_pbs.PathCurve == (byte)Extrusion.Straight)
		544	{
		545	volume *= 0.78539816339f; // elipse base
		546
		547	if (hollowAmount > 0.0)
		548	{
		549	switch (_pbs.HollowShape)
		550	{
		551	case HollowShape.Same:
		552	case HollowShape.Circle:
		553	break;
		554
		555	case HollowShape.Square:
		556	hollowVolume *= 0.5f * 2.5984480504799f;
		557	break;
		558
		559	case HollowShape.Triangle:
		560	hollowVolume *= .5f * 1.27323954473516f;
		561	break;
		562
		563	default:
		564	hollowVolume = 0;
		565	break;
		566	}
		567	volume *= (1.0f - hollowVolume);
		568	}
		569	}
		570
		571	else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
		572	{
		573	volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
		574	tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
		575	volume *= (1.0f - tmp * tmp);
		576
		577	if (hollowAmount > 0.0)
		578	{
		579
		580	// calculate the hollow volume by it's shape compared to the prim shape
		581	hollowVolume *= hollowAmount;
		582
		583	switch (_pbs.HollowShape)
		584	{
		585	case HollowShape.Same:
		586	case HollowShape.Circle:
		587	break;
		588
		589	case HollowShape.Square:
		590	hollowVolume *= 0.5f * 2.5984480504799f;
		591	break;
		592
		593	case HollowShape.Triangle:
		594	hollowVolume *= .5f * 1.27323954473516f;
		595	break;
		596
		597	default:
		598	hollowVolume = 0;
		599	break;
		600	}
		601	volume *= (1.0f - hollowVolume);
		602	}
		603	}
		604	break;
		605
		606	case ProfileShape.HalfCircle:
		607	if (_pbs.PathCurve == (byte)Extrusion.Curve1)
		608	{
		609	volume *= 0.52359877559829887307710723054658f;
		610	}
		611	break;
		612
		613	case ProfileShape.EquilateralTriangle:
		614
		615	if (_pbs.PathCurve == (byte)Extrusion.Straight)
		616	{
		617	volume *= 0.32475953f;
		618
		619	if (hollowAmount > 0.0)
		620	{
		621
		622	// calculate the hollow volume by it's shape compared to the prim shape
		623	switch (_pbs.HollowShape)
		624	{
		625	case HollowShape.Same:
		626	case HollowShape.Triangle:
		627	hollowVolume *= .25f;
		628	break;
		629
		630	case HollowShape.Square:
		631	hollowVolume *= 0.499849f * 3.07920140172638f;
		632	break;
		633
		634	case HollowShape.Circle:
		635	// Hollow shape is a perfect cyllinder in respect to the cube's scale
		636	// Cyllinder hollow volume calculation
		637
		638	hollowVolume *= 0.1963495f * 3.07920140172638f;
		639	break;
		640
		641	default:
		642	hollowVolume = 0;
		643	break;
		644	}
		645	volume *= (1.0f - hollowVolume);
		646	}
		647	}
		648	else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
		649	{
		650	volume *= 0.32475953f;
		651	volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
		652	tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
		653	volume *= (1.0f - tmp * tmp);
		654
		655	if (hollowAmount > 0.0)
		656	{
		657
		658	hollowVolume *= hollowAmount;
		659
		660	switch (_pbs.HollowShape)
		661	{
		662	case HollowShape.Same:
		663	case HollowShape.Triangle:
		664	hollowVolume *= .25f;
		665	break;
		666
		667	case HollowShape.Square:
		668	hollowVolume *= 0.499849f * 3.07920140172638f;
		669	break;
		670
		671	case HollowShape.Circle:
		672
		673	hollowVolume *= 0.1963495f * 3.07920140172638f;
		674	break;
		675
		676	default:
		677	hollowVolume = 0;
		678	break;
		679	}
		680	volume *= (1.0f - hollowVolume);
		681	}
		682	}
		683	break;
		684
		685	default:
		686	break;
		687	}
		688
		689	float taperX1;
		690	float taperY1;
		691	float taperX;
		692	float taperY;
		693	float pathBegin;
		694	float pathEnd;
		695	float profileBegin;
		696	float profileEnd;
		697
		698	if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
		699	{
		700	taperX1 = _pbs.PathScaleX * 0.01f;
		701	if (taperX1 > 1.0f)
		702	taperX1 = 2.0f - taperX1;
		703	taperX = 1.0f - taperX1;
		704
		705	taperY1 = _pbs.PathScaleY * 0.01f;
		706	if (taperY1 > 1.0f)
		707	taperY1 = 2.0f - taperY1;
		708	taperY = 1.0f - taperY1;
		709	}
		710	else
		711	{
		712	taperX = _pbs.PathTaperX * 0.01f;
		713	if (taperX < 0.0f)
		714	taperX = -taperX;
		715	taperX1 = 1.0f - taperX;
		716
		717	taperY = _pbs.PathTaperY * 0.01f;
		718	if (taperY < 0.0f)
		719	taperY = -taperY;
		720	taperY1 = 1.0f - taperY;
		721	}
		722
		723	volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
		724
		725	pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
		726	pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
		727	volume *= (pathEnd - pathBegin);
		728
		729	// this is crude aproximation
		730	profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
		731	profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
		732	volume *= (profileEnd - profileBegin);
		733
		734	returnMass = m_density * volume;
		735
		736	if (returnMass <= 0)
		737	returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
		738	// else if (returnMass > _parent_scene.maximumMassObject)
		739	// returnMass = _parent_scene.maximumMassObject;
		740
		741	// Recursively calculate mass
		742	bool HasChildPrim = false;
		743	lock (childrenPrim)
		744	{
		745	if (childrenPrim.Count > 0)
		746	{
		747	HasChildPrim = true;
		748	}
		749	}
		750
		751	if (HasChildPrim)
		752	{
		753	OdePrim[] childPrimArr = new OdePrim[0];
		754
		755	lock (childrenPrim)
		756	childPrimArr = childrenPrim.ToArray();
		757
		758	for (int i = 0; i < childPrimArr.Length; i++)
		759	{
		760	if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
		761	returnMass += childPrimArr[i].CalculateMass();
		762	// failsafe, this shouldn't happen but with OpenSim, you never know :)
		763	if (i > 256)
		764	break;
		765	}
		766	}
		767
		768	if (returnMass > _parent_scene.maximumMassObject)
		769	returnMass = _parent_scene.maximumMassObject;
		770
		771	return returnMass;
		772	}
		773
		774	#endregion
		775
		776	private void setMass()
		777	{
		778	if (Body != (IntPtr) 0)
		779	{
		780	float newmass = CalculateMass();
		781
		782	//m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
		783
		784	d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
		785	d.BodySetMass(Body, ref pMass);
		786	}
		787	}
		788
		789	/// <summary>
		790	/// Stop a prim from being subject to physics.
		791	/// </summary>
		792	internal void disableBody()
		793	{
		794	//this kills the body so things like 'mesh' can re-create it.
		795	lock (this)
		796	{
		797	if (!childPrim)
		798	{
		799	if (Body != IntPtr.Zero)
		800	{
		801	_parent_scene.DeactivatePrim(this);
		802	m_collisionCategories &= ~CollisionCategories.Body;
		803	m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
		804
		805	if (m_assetFailed)
		806	{
		807	d.GeomSetCategoryBits(prim_geom, 0);
		808	d.GeomSetCollideBits(prim_geom, 0);
		809	}
		810	else
		811	{
		812	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		813	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		814	}
		815
		816	d.BodyDestroy(Body);
		817	lock (childrenPrim)
		818	{
		819	if (childrenPrim.Count > 0)
		820	{
		821	foreach (OdePrim prm in childrenPrim)
		822	{
		823	_parent_scene.DeactivatePrim(prm);
		824	prm.Body = IntPtr.Zero;
		825	}
		826	}
		827	}
		828	Body = IntPtr.Zero;
		829	}
		830	}
		831	else
		832	{
		833	_parent_scene.DeactivatePrim(this);
		834
		835	m_collisionCategories &= ~CollisionCategories.Body;
		836	m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
		837
		838	if (m_assetFailed)
		839	{
		840	d.GeomSetCategoryBits(prim_geom, 0);
		841	d.GeomSetCollideBits(prim_geom, 0);
		842	}
		843	else
		844	{
		845
		846	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		847	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		848	}
		849
		850	Body = IntPtr.Zero;
		851	}
		852	}
		853
		854	m_disabled = true;
		855	m_collisionscore = 0;
		856	}
		857
		858	private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
		859
		860	private void setMesh(OdeScene parent_scene, IMesh mesh)
		861	{
		862	// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
		863
		864	// This sleeper is there to moderate how long it takes between
		865	// setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
		866
		867	//Thread.Sleep(10);
		868
		869	//Kill Body so that mesh can re-make the geom
		870	if (IsPhysical && Body != IntPtr.Zero)
		871	{
		872	if (childPrim)
		873	{
		874	if (_parent != null)
		875	{
		876	OdePrim parent = (OdePrim)_parent;
		877	parent.ChildDelink(this);
		878	}
		879	}
		880	else
		881	{
		882	disableBody();
		883	}
		884	}
		885
		886	IntPtr vertices, indices;
		887	int vertexCount, indexCount;
		888	int vertexStride, triStride;
		889	mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
		890	mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
		891	m_expectedCollisionContacts = indexCount;
		892	mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
		893
		894	// We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
		895	// the same time.
		896	lock (m_MeshToTriMeshMap)
		897	{
		898	if (m_MeshToTriMeshMap.ContainsKey(mesh))
		899	{
		900	_triMeshData = m_MeshToTriMeshMap[mesh];
		901	}
		902	else
		903	{
		904	_triMeshData = d.GeomTriMeshDataCreate();
		905
		906	d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
		907	d.GeomTriMeshDataPreprocess(_triMeshData);
		908	m_MeshToTriMeshMap[mesh] = _triMeshData;
		909	}
		910	}
		911
		912	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		913	try
		914	{
		915	SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
		916	}
		917	catch (AccessViolationException)
		918	{
		919	m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
		920	return;
		921	}
		922
		923	// if (IsPhysical && Body == (IntPtr) 0)
		924	// {
		925	// Recreate the body
		926	// m_interpenetrationcount = 0;
		927	// m_collisionscore = 0;
		928
		929	// enableBody();
		930	// }
		931	}
		932
		933	internal void ProcessTaints()
		934	{
		935	#if SPAM
		936	Console.WriteLine("ZProcessTaints for " + Name);
		937	#endif
		938
		939	// This must be processed as the very first taint so that later operations have a prim_geom to work with
		940	// if this is a new prim.
		941	if (m_taintadd)
		942	changeadd();
		943
		944	if (!_position.ApproxEquals(m_taintposition, 0f))
		945	changemove();
		946
		947	if (m_taintrot != _orientation)
		948	{
		949	if (childPrim && IsPhysical) // For physical child prim...
		950	{
		951	rotate();
		952	// KF: ODE will also rotate the parent prim!
		953	// so rotate the root back to where it was
		954	OdePrim parent = (OdePrim)_parent;
		955	parent.rotate();
		956	}
		957	else
		958	{
		959	//Just rotate the prim
		960	rotate();
		961	}
		962	}
		963
		964	if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
		965	changePhysicsStatus();
		966
		967	if (!_size.ApproxEquals(m_taintsize, 0f))
		968	changesize();
		969
		970	if (m_taintshape)
		971	changeshape();
		972
		973	if (m_taintforce)
		974	changeAddForce();
		975
		976	if (m_taintaddangularforce)
		977	changeAddAngularForce();
		978
		979	if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
		980	changeSetTorque();
		981
		982	if (m_taintdisable)
		983	changedisable();
		984
		985	if (m_taintselected != m_isSelected)
		986	changeSelectedStatus();
		987
		988	if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
		989	changevelocity();
		990
		991	if (m_taintparent != _parent)
		992	changelink();
		993
		994	if (m_taintCollidesWater != m_collidesWater)
		995	changefloatonwater();
		996
		997	if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
		998	changeAngularLock();
		999	}
		1000
		1001	/// <summary>
		1002	/// Change prim in response to an angular lock taint.
		1003	/// </summary>
		1004	private void changeAngularLock()
		1005	{
		1006	// do we have a Physical object?
		1007	if (Body != IntPtr.Zero)
		1008	{
		1009	//Check that we have a Parent
		1010	//If we have a parent then we're not authorative here
		1011	if (_parent == null)
		1012	{
		1013	if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
		1014	{
		1015	//d.BodySetFiniteRotationMode(Body, 0);
		1016	//d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
		1017	createAMotor(m_taintAngularLock);
		1018	}
		1019	else
		1020	{
		1021	if (Amotor != IntPtr.Zero)
		1022	{
		1023	d.JointDestroy(Amotor);
		1024	Amotor = IntPtr.Zero;
		1025	}
		1026	}
		1027	}
		1028	}
		1029
		1030	// Store this for later in case we get turned into a separate body
		1031	m_angularlock = m_taintAngularLock;
		1032	}
		1033
		1034	/// <summary>
		1035	/// Change prim in response to a link taint.
		1036	/// </summary>
		1037	private void changelink()
		1038	{
		1039	// If the newly set parent is not null
		1040	// create link
		1041	if (_parent == null && m_taintparent != null)
		1042	{
		1043	if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
		1044	{
		1045	OdePrim obj = (OdePrim)m_taintparent;
		1046	//obj.disableBody();
		1047	//Console.WriteLine("changelink calls ParentPrim");
		1048	obj.AddChildPrim(this);
		1049
		1050	/*
		1051	if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
		1052	{
		1053	_linkJointGroup = d.JointGroupCreate(0);
		1054	m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
		1055	d.JointAttach(m_linkJoint, obj.Body, Body);
		1056	d.JointSetFixed(m_linkJoint);
		1057	}
		1058	*/
		1059	}
		1060	}
		1061	// If the newly set parent is null
		1062	// destroy link
		1063	else if (_parent != null && m_taintparent == null)
		1064	{
		1065	//Console.WriteLine(" changelink B");
		1066
		1067	if (_parent is OdePrim)
		1068	{
		1069	OdePrim obj = (OdePrim)_parent;
		1070	obj.ChildDelink(this);
		1071	childPrim = false;
		1072	//_parent = null;
		1073	}
		1074
		1075	/*
		1076	if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
		1077	d.JointGroupDestroy(_linkJointGroup);
		1078
		1079	_linkJointGroup = (IntPtr)0;
		1080	m_linkJoint = (IntPtr)0;
		1081	*/
		1082	}
		1083
		1084	_parent = m_taintparent;
		1085	m_taintPhysics = IsPhysical;
		1086	}
		1087
		1088	/// <summary>
		1089	/// Add a child prim to this parent prim.
		1090	/// </summary>
		1091	/// <param name="prim">Child prim</param>
		1092	private void AddChildPrim(OdePrim prim)
		1093	{
		1094	if (LocalID == prim.LocalID)
		1095	return;
		1096
		1097	if (Body == IntPtr.Zero)
		1098	{
		1099	Body = d.BodyCreate(_parent_scene.world);
		1100	setMass();
		1101	}
		1102
		1103	lock (childrenPrim)
		1104	{
		1105	if (childrenPrim.Contains(prim))
		1106	return;
		1107
		1108	// m_log.DebugFormat(
		1109	// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
		1110
		1111	childrenPrim.Add(prim);
		1112
		1113	foreach (OdePrim prm in childrenPrim)
		1114	{
		1115	d.Mass m2;
		1116	d.MassSetZero(out m2);
		1117	d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
		1118
		1119	d.Quaternion quat = new d.Quaternion();
		1120	quat.W = prm._orientation.W;
		1121	quat.X = prm._orientation.X;
		1122	quat.Y = prm._orientation.Y;
		1123	quat.Z = prm._orientation.Z;
		1124
		1125	d.Matrix3 mat = new d.Matrix3();
		1126	d.RfromQ(out mat, ref quat);
		1127	d.MassRotate(ref m2, ref mat);
		1128	d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
		1129	d.MassAdd(ref pMass, ref m2);
		1130	}
		1131
		1132	foreach (OdePrim prm in childrenPrim)
		1133	{
		1134	prm.m_collisionCategories |= CollisionCategories.Body;
		1135	prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
		1136
		1137	//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
		1138	if (prm.m_assetFailed)
		1139	{
		1140	d.GeomSetCategoryBits(prm.prim_geom, 0);
		1141	d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
		1142	}
		1143	else
		1144	{
		1145	d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
		1146	d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
		1147	}
		1148
		1149	d.Quaternion quat = new d.Quaternion();
		1150	quat.W = prm._orientation.W;
		1151	quat.X = prm._orientation.X;
		1152	quat.Y = prm._orientation.Y;
		1153	quat.Z = prm._orientation.Z;
		1154
		1155	d.Matrix3 mat = new d.Matrix3();
		1156	d.RfromQ(out mat, ref quat);
		1157	if (Body != IntPtr.Zero)
		1158	{
		1159	d.GeomSetBody(prm.prim_geom, Body);
		1160	prm.childPrim = true;
		1161	d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
		1162	//d.GeomSetOffsetPosition(prim.prim_geom,
		1163	// (Position.X - prm.Position.X) - pMass.c.X,
		1164	// (Position.Y - prm.Position.Y) - pMass.c.Y,
		1165	// (Position.Z - prm.Position.Z) - pMass.c.Z);
		1166	d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
		1167	//d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
		1168	d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
		1169	d.BodySetMass(Body, ref pMass);
		1170	}
		1171	else
		1172	{
		1173	m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
		1174	}
		1175
		1176	prm.m_interpenetrationcount = 0;
		1177	prm.m_collisionscore = 0;
		1178	prm.m_disabled = false;
		1179
		1180	// The body doesn't already have a finite rotation mode set here
		1181	if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
		1182	{
		1183	prm.createAMotor(m_angularlock);
		1184	}
		1185	prm.Body = Body;
		1186	_parent_scene.ActivatePrim(prm);
		1187	}
		1188
		1189	m_collisionCategories |= CollisionCategories.Body;
		1190	m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
		1191
		1192	if (m_assetFailed)
		1193	{
		1194	d.GeomSetCategoryBits(prim_geom, 0);
		1195	d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
		1196	}
		1197	else
		1198	{
		1199	//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
		1200	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		1201	//Console.WriteLine(" Post GeomSetCategoryBits 2");
		1202	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		1203	}
		1204
		1205	d.Quaternion quat2 = new d.Quaternion();
		1206	quat2.W = _orientation.W;
		1207	quat2.X = _orientation.X;
		1208	quat2.Y = _orientation.Y;
		1209	quat2.Z = _orientation.Z;
		1210
		1211	d.Matrix3 mat2 = new d.Matrix3();
		1212	d.RfromQ(out mat2, ref quat2);
		1213	d.GeomSetBody(prim_geom, Body);
		1214	d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
		1215	//d.GeomSetOffsetPosition(prim.prim_geom,
		1216	// (Position.X - prm.Position.X) - pMass.c.X,
		1217	// (Position.Y - prm.Position.Y) - pMass.c.Y,
		1218	// (Position.Z - prm.Position.Z) - pMass.c.Z);
		1219	//d.GeomSetOffsetRotation(prim_geom, ref mat2);
		1220	d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
		1221	d.BodySetMass(Body, ref pMass);
		1222
		1223	d.BodySetAutoDisableFlag(Body, true);
		1224	d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
		1225
		1226	m_interpenetrationcount = 0;
		1227	m_collisionscore = 0;
		1228	m_disabled = false;
		1229
		1230	// The body doesn't already have a finite rotation mode set here
		1231	if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
		1232	{
		1233	createAMotor(m_angularlock);
		1234	}
		1235
		1236	d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
		1237
		1238	if (m_vehicle.Type != Vehicle.TYPE_NONE)
		1239	m_vehicle.Enable(Body, _parent_scene);
		1240
		1241	_parent_scene.ActivatePrim(this);
		1242	}
		1243	}
		1244
		1245	private void ChildSetGeom(OdePrim odePrim)
		1246	{
		1247	// m_log.DebugFormat(
		1248	// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
		1249
		1250	//if (IsPhysical && Body != IntPtr.Zero)
		1251	lock (childrenPrim)
		1252	{
		1253	foreach (OdePrim prm in childrenPrim)
		1254	{
		1255	//prm.childPrim = true;
		1256	prm.disableBody();
		1257	//prm.m_taintparent = null;
		1258	//prm._parent = null;
		1259	//prm.m_taintPhysics = false;
		1260	//prm.m_disabled = true;
		1261	//prm.childPrim = false;
		1262	}
		1263	}
		1264
		1265	disableBody();
		1266
		1267	// Spurious - Body == IntPtr.Zero after disableBody()
		1268	// if (Body != IntPtr.Zero)
		1269	// {
		1270	// _parent_scene.DeactivatePrim(this);
		1271	// }
		1272
		1273	lock (childrenPrim)
		1274	{
		1275	foreach (OdePrim prm in childrenPrim)
		1276	{
		1277	//Console.WriteLine("ChildSetGeom calls ParentPrim");
		1278	AddChildPrim(prm);
		1279	}
		1280	}
		1281	}
		1282
		1283	private void ChildDelink(OdePrim odePrim)
		1284	{
		1285	// m_log.DebugFormat(
		1286	// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
		1287
		1288	// Okay, we have a delinked child.. need to rebuild the body.
		1289	lock (childrenPrim)
		1290	{
		1291	foreach (OdePrim prm in childrenPrim)
		1292	{
		1293	prm.childPrim = true;
		1294	prm.disableBody();
		1295	//prm.m_taintparent = null;
		1296	//prm._parent = null;
		1297	//prm.m_taintPhysics = false;
		1298	//prm.m_disabled = true;
		1299	//prm.childPrim = false;
		1300	}
		1301	}
		1302
		1303	disableBody();
		1304
		1305	lock (childrenPrim)
		1306	{
		1307	//Console.WriteLine("childrenPrim.Remove " + odePrim);
		1308	childrenPrim.Remove(odePrim);
		1309	}
		1310
		1311	// Spurious - Body == IntPtr.Zero after disableBody()
		1312	// if (Body != IntPtr.Zero)
		1313	// {
		1314	// _parent_scene.DeactivatePrim(this);
		1315	// }
		1316
		1317	lock (childrenPrim)
		1318	{
		1319	foreach (OdePrim prm in childrenPrim)
		1320	{
		1321	//Console.WriteLine("ChildDelink calls ParentPrim");
		1322	AddChildPrim(prm);
		1323	}
		1324	}
		1325	}
		1326
		1327	/// <summary>
		1328	/// Change prim in response to a selection taint.
		1329	/// </summary>
		1330	private void changeSelectedStatus()
		1331	{
		1332	if (m_taintselected)
		1333	{
		1334	m_collisionCategories = CollisionCategories.Selected;
		1335	m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
		1336
		1337	// We do the body disable soft twice because 'in theory' a collision could have happened
		1338	// in between the disabling and the collision properties setting
		1339	// which would wake the physical body up from a soft disabling and potentially cause it to fall
		1340	// through the ground.
		1341
		1342	// NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
		1343	// just one part of the assembly, the rest of the assembly is non-selected and still simulating,
		1344	// so that causes the selected part to wake up and continue moving.
		1345
		1346	// even if you select all parts of a jointed assembly, it is not guaranteed that the entire
		1347	// assembly will stop simulating during the selection, because of the lack of atomicity
		1348	// of select operations (their processing could be interrupted by a thread switch, causing
		1349	// simulation to continue before all of the selected object notifications trickle down to
		1350	// the physics engine).
		1351
		1352	// e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
		1353	// selected and disabled. then, due to a thread switch, the selection processing is
		1354	// interrupted and the physics engine continues to simulate, so the last 50 items, whose
		1355	// selection was not yet processed, continues to simulate. this wakes up ALL of the
		1356	// first 50 again. then the last 50 are disabled. then the first 50, which were just woken
		1357	// up, start simulating again, which in turn wakes up the last 50.
		1358
		1359	if (IsPhysical)
		1360	{
		1361	disableBodySoft();
		1362	}
		1363
		1364	if (m_assetFailed)
		1365	{
		1366	d.GeomSetCategoryBits(prim_geom, 0);
		1367	d.GeomSetCollideBits(prim_geom, 0);
		1368	}
		1369	else
		1370	{
		1371	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		1372	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		1373	}
		1374
		1375	if (IsPhysical)
		1376	{
		1377	disableBodySoft();
		1378	}
		1379	}
		1380	else
		1381	{
		1382	m_collisionCategories = CollisionCategories.Geom;
		1383
		1384	if (IsPhysical)
		1385	m_collisionCategories |= CollisionCategories.Body;
		1386
		1387	m_collisionFlags = m_default_collisionFlags;
		1388
		1389	if (m_collidesLand)
		1390	m_collisionFlags |= CollisionCategories.Land;
		1391	if (m_collidesWater)
		1392	m_collisionFlags |= CollisionCategories.Water;
		1393
		1394	if (m_assetFailed)
		1395	{
		1396	d.GeomSetCategoryBits(prim_geom, 0);
		1397	d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
		1398	}
		1399	else
		1400	{
		1401	d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
		1402	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		1403	}
		1404
		1405	if (IsPhysical)
		1406	{
		1407	if (Body != IntPtr.Zero)
		1408	{
		1409	d.BodySetLinearVel(Body, 0f, 0f, 0f);
		1410	d.BodySetForce(Body, 0, 0, 0);
		1411	enableBodySoft();
		1412	}
		1413	}
		1414	}
		1415
		1416	resetCollisionAccounting();
		1417	m_isSelected = m_taintselected;
		1418	}//end changeSelectedStatus
		1419
		1420	internal void ResetTaints()
		1421	{
		1422	m_taintposition = _position;
		1423	m_taintrot = _orientation;
		1424	m_taintPhysics = IsPhysical;
		1425	m_taintselected = m_isSelected;
		1426	m_taintsize = _size;
		1427	m_taintshape = false;
		1428	m_taintforce = false;
		1429	m_taintdisable = false;
		1430	m_taintVelocity = Vector3.Zero;
		1431	}
		1432
		1433	/// <summary>
		1434	/// Create a geometry for the given mesh in the given target space.
		1435	/// </summary>
		1436	/// <param name="m_targetSpace"></param>
		1437	/// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
		1438	private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
		1439	{
		1440	#if SPAM
		1441	Console.WriteLine("CreateGeom:");
		1442	#endif
		1443	if (mesh != null)
		1444	{
		1445	setMesh(_parent_scene, mesh);
		1446	}
		1447	else
		1448	{
		1449	if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
		1450	{
		1451	if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
		1452	{
		1453	if (((_size.X / 2f) > 0f))
		1454	{
		1455	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1456	try
		1457	{
		1458	//Console.WriteLine(" CreateGeom 1");
		1459	SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
		1460	m_expectedCollisionContacts = 3;
		1461	}
		1462	catch (AccessViolationException)
		1463	{
		1464	m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
		1465	return;
		1466	}
		1467	}
		1468	else
		1469	{
		1470	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1471	try
		1472	{
		1473	//Console.WriteLine(" CreateGeom 2");
		1474	SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
		1475	m_expectedCollisionContacts = 4;
		1476	}
		1477	catch (AccessViolationException)
		1478	{
		1479	m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
		1480	return;
		1481	}
		1482	}
		1483	}
		1484	else
		1485	{
		1486	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1487	try
		1488	{
		1489	//Console.WriteLine(" CreateGeom 3");
		1490	SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
		1491	m_expectedCollisionContacts = 4;
		1492	}
		1493	catch (AccessViolationException)
		1494	{
		1495	m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
		1496	return;
		1497	}
		1498	}
		1499	}
		1500	else
		1501	{
		1502	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1503	try
		1504	{
		1505	//Console.WriteLine(" CreateGeom 4");
		1506	SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
		1507	m_expectedCollisionContacts = 4;
		1508	}
		1509	catch (AccessViolationException)
		1510	{
		1511	m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
		1512	return;
		1513	}
		1514	}
		1515	}
		1516	}
		1517
		1518	/// <summary>
		1519	/// Remove the existing geom from this prim.
		1520	/// </summary>
		1521	/// <param name="m_targetSpace"></param>
		1522	/// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
		1523	/// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
		1524	internal bool RemoveGeom()
		1525	{
		1526	if (prim_geom != IntPtr.Zero)
		1527	{
		1528	try
		1529	{
		1530	_parent_scene.geom_name_map.Remove(prim_geom);
		1531	_parent_scene.actor_name_map.Remove(prim_geom);
		1532	d.GeomDestroy(prim_geom);
		1533	m_expectedCollisionContacts = 0;
		1534	prim_geom = IntPtr.Zero;
		1535	}
		1536	catch (System.AccessViolationException)
		1537	{
		1538	prim_geom = IntPtr.Zero;
		1539	m_expectedCollisionContacts = 0;
		1540	m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
		1541
		1542	return false;
		1543	}
		1544
		1545	return true;
		1546	}
		1547	else
		1548	{
		1549	m_log.WarnFormat(
		1550	"[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
		1551
		1552	return false;
		1553	}
		1554	}
		1555	/// <summary>
		1556	/// Add prim in response to an add taint.
		1557	/// </summary>
		1558	private void changeadd()
		1559	{
		1560	// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
		1561
		1562	int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
		1563	IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
		1564
		1565	if (targetspace == IntPtr.Zero)
		1566	targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
		1567
		1568	m_targetSpace = targetspace;
		1569
		1570	IMesh mesh = null;
		1571
		1572	if (_parent_scene.needsMeshing(_pbs))
		1573	{
		1574	// Don't need to re-enable body.. it's done in SetMesh
		1575	mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
		1576	// createmesh returns null when it's a shape that isn't a cube.
		1577	// m_log.Debug(m_localID);
		1578	if (mesh == null)
		1579	CheckMeshAsset();
		1580	else
		1581	m_assetFailed = false;
		1582	}
		1583
		1584	#if SPAM
		1585	Console.WriteLine("changeadd 1");
		1586	#endif
		1587	CreateGeom(m_targetSpace, mesh);
		1588
		1589	d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
		1590	d.Quaternion myrot = new d.Quaternion();
		1591	myrot.X = _orientation.X;
		1592	myrot.Y = _orientation.Y;
		1593	myrot.Z = _orientation.Z;
		1594	myrot.W = _orientation.W;
		1595	d.GeomSetQuaternion(prim_geom, ref myrot);
		1596
		1597	if (IsPhysical && Body == IntPtr.Zero)
		1598	enableBody();
		1599
		1600	changeSelectedStatus();
		1601
		1602	m_taintadd = false;
		1603	}
		1604
		1605	/// <summary>
		1606	/// Move prim in response to a move taint.
		1607	/// </summary>
		1608	private void changemove()
		1609	{
		1610	if (IsPhysical)
		1611	{
		1612	if (!m_disabled && !m_taintremove && !childPrim)
		1613	{
		1614	if (Body == IntPtr.Zero)
		1615	enableBody();
		1616
		1617	//Prim auto disable after 20 frames,
		1618	//if you move it, re-enable the prim manually.
		1619	if (_parent != null)
		1620	{
		1621	if (m_linkJoint != IntPtr.Zero)
		1622	{
		1623	d.JointDestroy(m_linkJoint);
		1624	m_linkJoint = IntPtr.Zero;
		1625	}
		1626	}
		1627
		1628	if (Body != IntPtr.Zero)
		1629	{
		1630	d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
		1631
		1632	if (_parent != null)
		1633	{
		1634	OdePrim odParent = (OdePrim)_parent;
		1635	if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
		1636	{
		1637	// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
		1638	Console.WriteLine(" JointCreateFixed");
		1639	m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
		1640	d.JointAttach(m_linkJoint, Body, odParent.Body);
		1641	d.JointSetFixed(m_linkJoint);
		1642	}
		1643	}
		1644	d.BodyEnable(Body);
		1645	if (m_vehicle.Type != Vehicle.TYPE_NONE)
		1646	{
		1647	m_vehicle.Enable(Body, _parent_scene);
		1648	}
		1649	}
		1650	else
		1651	{
		1652	m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
		1653	}
		1654	}
		1655	//else
		1656	// {
		1657	//m_log.Debug("[BUG]: race!");
		1658	//}
		1659	}
		1660
		1661	// string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
		1662	// int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
		1663	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1664
		1665	IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
		1666	m_targetSpace = tempspace;
		1667
		1668	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1669
		1670	d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
		1671
		1672	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		1673	d.SpaceAdd(m_targetSpace, prim_geom);
		1674
		1675	changeSelectedStatus();
		1676
		1677	resetCollisionAccounting();
		1678	m_taintposition = _position;
		1679	}
		1680
		1681	internal void Move(float timestep)
		1682	{
		1683	float fx = 0;
		1684	float fy = 0;
		1685	float fz = 0;
		1686
		1687	if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
		1688	{
		1689	if (m_vehicle.Type != Vehicle.TYPE_NONE)
		1690	{
		1691	// 'VEHICLES' are dealt with in ODEDynamics.cs
		1692	m_vehicle.Step(timestep, _parent_scene);
		1693	}
		1694	else
		1695	{
		1696	//Console.WriteLine("Move " + Name);
		1697	if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
		1698	// NON-'VEHICLES' are dealt with here
		1699	// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
		1700	// {
		1701	// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
		1702	// /*
		1703	// if (m_angularlock.X == 1)
		1704	// avel2.X = 0;
		1705	// if (m_angularlock.Y == 1)
		1706	// avel2.Y = 0;
		1707	// if (m_angularlock.Z == 1)
		1708	// avel2.Z = 0;
		1709	// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
		1710	// */
		1711	// }
		1712	//float PID_P = 900.0f;
		1713
		1714	float m_mass = CalculateMass();
		1715
		1716	// fz = 0f;
		1717	//m_log.Info(m_collisionFlags.ToString());
		1718
		1719
		1720	//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
		1721	// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
		1722	// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
		1723	// gravityz multiplier = 1 - m_buoyancy
		1724	fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
		1725
		1726	if (m_usePID)
		1727	{
		1728	//Console.WriteLine("PID " + Name);
		1729	// KF - this is for object move? eg. llSetPos() ?
		1730	//if (!d.BodyIsEnabled(Body))
		1731	//d.BodySetForce(Body, 0f, 0f, 0f);
		1732	// If we're using the PID controller, then we have no gravity
		1733	//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
		1734	fz = 0f;
		1735
		1736	// no lock; for now it's only called from within Simulate()
		1737
		1738	// If the PID Controller isn't active then we set our force
		1739	// calculating base velocity to the current position
		1740
		1741	if ((m_PIDTau < 1) && (m_PIDTau != 0))
		1742	{
		1743	//PID_G = PID_G / m_PIDTau;
		1744	m_PIDTau = 1;
		1745	}
		1746
		1747	if ((PID_G - m_PIDTau) <= 0)
		1748	{
		1749	PID_G = m_PIDTau + 1;
		1750	}
		1751	//PidStatus = true;
		1752
		1753	// PhysicsVector vec = new PhysicsVector();
		1754	d.Vector3 vel = d.BodyGetLinearVel(Body);
		1755
		1756	d.Vector3 pos = d.BodyGetPosition(Body);
		1757	_target_velocity =
		1758	new Vector3(
		1759	(m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
		1760	(m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
		1761	(m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
		1762	);
		1763
		1764	// if velocity is zero, use position control; otherwise, velocity control
		1765
		1766	if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
		1767	{
		1768	// keep track of where we stopped. No more slippin' & slidin'
		1769
		1770	// We only want to deactivate the PID Controller if we think we want to have our surrogate
		1771	// react to the physics scene by moving it's position.
		1772	// Avatar to Avatar collisions
		1773	// Prim to avatar collisions
		1774
		1775	//fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
		1776	//fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
		1777	//fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
		1778	d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
		1779	d.BodySetLinearVel(Body, 0, 0, 0);
		1780	d.BodyAddForce(Body, 0, 0, fz);
		1781	return;
		1782	}
		1783	else
		1784	{
		1785	_zeroFlag = false;
		1786
		1787	// We're flying and colliding with something
		1788	fx = ((_target_velocity.X) - vel.X) * (PID_D);
		1789	fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
		1790
		1791	// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
		1792
		1793	fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
		1794	}
		1795	} // end if (m_usePID)
		1796
		1797	// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
		1798	if (m_useHoverPID && !m_usePID)
		1799	{
		1800	//Console.WriteLine("Hover " + Name);
		1801
		1802	// If we're using the PID controller, then we have no gravity
		1803	fz = (-1 * _parent_scene.gravityz) * m_mass;
		1804
		1805	// no lock; for now it's only called from within Simulate()
		1806
		1807	// If the PID Controller isn't active then we set our force
		1808	// calculating base velocity to the current position
		1809
		1810	if ((m_PIDTau < 1))
		1811	{
		1812	PID_G = PID_G / m_PIDTau;
		1813	}
		1814
		1815	if ((PID_G - m_PIDTau) <= 0)
		1816	{
		1817	PID_G = m_PIDTau + 1;
		1818	}
		1819
		1820	// Where are we, and where are we headed?
		1821	d.Vector3 pos = d.BodyGetPosition(Body);
		1822	d.Vector3 vel = d.BodyGetLinearVel(Body);
		1823
		1824	// Non-Vehicles have a limited set of Hover options.
		1825	// determine what our target height really is based on HoverType
		1826	switch (m_PIDHoverType)
		1827	{
		1828	case PIDHoverType.Ground:
		1829	m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
		1830	m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
		1831	break;
		1832	case PIDHoverType.GroundAndWater:
		1833	m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
		1834	m_waterHeight = _parent_scene.GetWaterLevel();
		1835	if (m_groundHeight > m_waterHeight)
		1836	{
		1837	m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
		1838	}
		1839	else
		1840	{
		1841	m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
		1842	}
		1843	break;
		1844
		1845	} // end switch (m_PIDHoverType)
		1846
		1847
		1848	_target_velocity =
		1849	new Vector3(0.0f, 0.0f,
		1850	(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
		1851	);
		1852
		1853	// if velocity is zero, use position control; otherwise, velocity control
		1854
		1855	if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
		1856	{
		1857	// keep track of where we stopped. No more slippin' & slidin'
		1858
		1859	// We only want to deactivate the PID Controller if we think we want to have our surrogate
		1860	// react to the physics scene by moving it's position.
		1861	// Avatar to Avatar collisions
		1862	// Prim to avatar collisions
		1863
		1864	d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
		1865	d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
		1866	d.BodyAddForce(Body, 0, 0, fz);
		1867	return;
		1868	}
		1869	else
		1870	{
		1871	_zeroFlag = false;
		1872
		1873	// We're flying and colliding with something
		1874	fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
		1875	}
		1876	}
		1877
		1878	fx *= m_mass;
		1879	fy *= m_mass;
		1880	//fz *= m_mass;
		1881
		1882	fx += m_force.X;
		1883	fy += m_force.Y;
		1884	fz += m_force.Z;
		1885
		1886	//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
		1887	if (fx != 0 || fy != 0 || fz != 0)
		1888	{
		1889	//m_taintdisable = true;
		1890	//base.RaiseOutOfBounds(Position);
		1891	//d.BodySetLinearVel(Body, fx, fy, 0f);
		1892	if (!d.BodyIsEnabled(Body))
		1893	{
		1894	// A physical body at rest on a surface will auto-disable after a while,
		1895	// this appears to re-enable it incase the surface it is upon vanishes,
		1896	// and the body should fall again.
		1897	d.BodySetLinearVel(Body, 0f, 0f, 0f);
		1898	d.BodySetForce(Body, 0, 0, 0);
		1899	enableBodySoft();
		1900	}
		1901
		1902	// 35x10 = 350n times the mass per second applied maximum.
		1903	float nmax = 35f * m_mass;
		1904	float nmin = -35f * m_mass;
		1905
		1906	if (fx > nmax)
		1907	fx = nmax;
		1908	if (fx < nmin)
		1909	fx = nmin;
		1910	if (fy > nmax)
		1911	fy = nmax;
		1912	if (fy < nmin)
		1913	fy = nmin;
		1914	d.BodyAddForce(Body, fx, fy, fz);
		1915	//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
		1916	}
		1917	}
		1918	}
		1919	else
		1920	{ // is not physical, or is not a body or is selected
		1921	// _zeroPosition = d.BodyGetPosition(Body);
		1922	return;
		1923	//Console.WriteLine("Nothing " + Name);
		1924
		1925	}
		1926	}
		1927
		1928	private void rotate()
		1929	{
		1930	d.Quaternion myrot = new d.Quaternion();
		1931	myrot.X = _orientation.X;
		1932	myrot.Y = _orientation.Y;
		1933	myrot.Z = _orientation.Z;
		1934	myrot.W = _orientation.W;
		1935	if (Body != IntPtr.Zero)
		1936	{
		1937	// KF: If this is a root prim do BodySet
		1938	d.BodySetQuaternion(Body, ref myrot);
		1939	if (IsPhysical)
		1940	{
		1941	if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
		1942	createAMotor(m_angularlock);
		1943	}
		1944	}
		1945	else
		1946	{
		1947	// daughter prim, do Geom set
		1948	d.GeomSetQuaternion(prim_geom, ref myrot);
		1949	}
		1950
		1951	resetCollisionAccounting();
		1952	m_taintrot = _orientation;
		1953	}
		1954
		1955	private void resetCollisionAccounting()
		1956	{
		1957	m_collisionscore = 0;
		1958	m_interpenetrationcount = 0;
		1959	m_disabled = false;
		1960	}
		1961
		1962	/// <summary>
		1963	/// Change prim in response to a disable taint.
		1964	/// </summary>
		1965	private void changedisable()
		1966	{
		1967	m_disabled = true;
		1968	if (Body != IntPtr.Zero)
		1969	{
		1970	d.BodyDisable(Body);
		1971	Body = IntPtr.Zero;
		1972	}
		1973
		1974	m_taintdisable = false;
		1975	}
		1976
		1977	/// <summary>
		1978	/// Change prim in response to a physics status taint
		1979	/// </summary>
		1980	private void changePhysicsStatus()
		1981	{
		1982	if (IsPhysical)
		1983	{
		1984	if (Body == IntPtr.Zero)
		1985	{
		1986	if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
		1987	{
		1988	changeshape();
		1989	}
		1990	else
		1991	{
		1992	enableBody();
		1993	}
		1994	}
		1995	}
		1996	else
		1997	{
		1998	if (Body != IntPtr.Zero)
		1999	{
		2000	if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
		2001	{
		2002	RemoveGeom();
		2003
		2004	//Console.WriteLine("changePhysicsStatus for " + Name);
		2005	changeadd();
		2006	}
		2007
		2008	if (childPrim)
		2009	{
		2010	if (_parent != null)
		2011	{
		2012	OdePrim parent = (OdePrim)_parent;
		2013	parent.ChildDelink(this);
		2014	}
		2015	}
		2016	else
		2017	{
		2018	disableBody();
		2019	}
		2020	}
		2021	}
		2022
		2023	changeSelectedStatus();
		2024
		2025	resetCollisionAccounting();
		2026	m_taintPhysics = IsPhysical;
		2027	}
		2028
		2029	/// <summary>
		2030	/// Change prim in response to a size taint.
		2031	/// </summary>
		2032	private void changesize()
		2033	{
		2034	#if SPAM
		2035	m_log.DebugFormat("[ODE PRIM]: Called changesize");
		2036	#endif
		2037
		2038	if (_size.X <= 0) _size.X = 0.01f;
		2039	if (_size.Y <= 0) _size.Y = 0.01f;
		2040	if (_size.Z <= 0) _size.Z = 0.01f;
		2041
		2042	//kill body to rebuild
		2043	if (IsPhysical && Body != IntPtr.Zero)
		2044	{
		2045	if (childPrim)
		2046	{
		2047	if (_parent != null)
		2048	{
		2049	OdePrim parent = (OdePrim)_parent;
		2050	parent.ChildDelink(this);
		2051	}
		2052	}
		2053	else
		2054	{
		2055	disableBody();
		2056	}
		2057	}
		2058
		2059	if (d.SpaceQuery(m_targetSpace, prim_geom))
		2060	{
		2061	// _parent_scene.waitForSpaceUnlock(m_targetSpace);
		2062	d.SpaceRemove(m_targetSpace, prim_geom);
		2063	}
		2064
		2065	RemoveGeom();
		2066
		2067	// we don't need to do space calculation because the client sends a position update also.
		2068
		2069	IMesh mesh = null;
		2070
		2071	// Construction of new prim
		2072	if (_parent_scene.needsMeshing(_pbs))
		2073	{
		2074	float meshlod = _parent_scene.meshSculptLOD;
		2075
		2076	if (IsPhysical)
		2077	meshlod = _parent_scene.MeshSculptphysicalLOD;
		2078	// Don't need to re-enable body.. it's done in SetMesh
		2079
		2080	if (_parent_scene.needsMeshing(_pbs))
		2081	{
		2082	mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
		2083	if (mesh == null)
		2084	CheckMeshAsset();
		2085	else
		2086	m_assetFailed = false;
		2087	}
		2088
		2089	}
		2090
		2091	CreateGeom(m_targetSpace, mesh);
		2092	d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
		2093	d.Quaternion myrot = new d.Quaternion();
		2094	myrot.X = _orientation.X;
		2095	myrot.Y = _orientation.Y;
		2096	myrot.Z = _orientation.Z;
		2097	myrot.W = _orientation.W;
		2098	d.GeomSetQuaternion(prim_geom, ref myrot);
		2099
		2100	//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
		2101	if (IsPhysical && Body == IntPtr.Zero && !childPrim)
		2102	{
		2103	// Re creates body on size.
		2104	// EnableBody also does setMass()
		2105	enableBody();
		2106	d.BodyEnable(Body);
		2107	}
		2108
		2109	changeSelectedStatus();
		2110
		2111	if (childPrim)
		2112	{
		2113	if (_parent is OdePrim)
		2114	{
		2115	OdePrim parent = (OdePrim)_parent;
		2116	parent.ChildSetGeom(this);
		2117	}
		2118	}
		2119	resetCollisionAccounting();
		2120	m_taintsize = _size;
		2121	}
		2122
		2123	/// <summary>
		2124	/// Change prim in response to a float on water taint.
		2125	/// </summary>
		2126	/// <param name="timestep"></param>
		2127	private void changefloatonwater()
		2128	{
		2129	m_collidesWater = m_taintCollidesWater;
		2130
		2131	if (m_collidesWater)
		2132	{
		2133	m_collisionFlags |= CollisionCategories.Water;
		2134	}
		2135	else
		2136	{
		2137	m_collisionFlags &= ~CollisionCategories.Water;
		2138	}
		2139
		2140	if (m_assetFailed)
		2141	d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
		2142	else
		2143
		2144	d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
		2145	}
		2146	/// <summary>
		2147	/// Change prim in response to a shape taint.
		2148	/// </summary>
		2149	private void changeshape()
		2150	{
		2151	m_taintshape = false;
		2152
		2153	// Cleanup of old prim geometry and Bodies
		2154	if (IsPhysical && Body != IntPtr.Zero)
		2155	{
		2156	if (childPrim)
		2157	{
		2158	if (_parent != null)
		2159	{
		2160	OdePrim parent = (OdePrim)_parent;
		2161	parent.ChildDelink(this);
		2162	}
		2163	}
		2164	else
		2165	{
		2166	disableBody();
		2167	}
		2168	}
		2169
		2170	RemoveGeom();
		2171
		2172	// we don't need to do space calculation because the client sends a position update also.
		2173	if (_size.X <= 0) _size.X = 0.01f;
		2174	if (_size.Y <= 0) _size.Y = 0.01f;
		2175	if (_size.Z <= 0) _size.Z = 0.01f;
		2176	// Construction of new prim
		2177
		2178	IMesh mesh = null;
		2179
		2180
		2181	if (_parent_scene.needsMeshing(_pbs))
		2182	{
		2183	// Don't need to re-enable body.. it's done in CreateMesh
		2184	float meshlod = _parent_scene.meshSculptLOD;
		2185
		2186	if (IsPhysical)
		2187	meshlod = _parent_scene.MeshSculptphysicalLOD;
		2188
		2189	// createmesh returns null when it doesn't mesh.
		2190	mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
		2191	if (mesh == null)
		2192	CheckMeshAsset();
		2193	else
		2194	m_assetFailed = false;
		2195	}
		2196
		2197	CreateGeom(m_targetSpace, mesh);
		2198	d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
		2199	d.Quaternion myrot = new d.Quaternion();
		2200	//myrot.W = _orientation.w;
		2201	myrot.W = _orientation.W;
		2202	myrot.X = _orientation.X;
		2203	myrot.Y = _orientation.Y;
		2204	myrot.Z = _orientation.Z;
		2205	d.GeomSetQuaternion(prim_geom, ref myrot);
		2206
		2207	//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
		2208	if (IsPhysical && Body == IntPtr.Zero)
		2209	{
		2210	// Re creates body on size.
		2211	// EnableBody also does setMass()
		2212	enableBody();
		2213	if (Body != IntPtr.Zero)
		2214	{
		2215	d.BodyEnable(Body);
		2216	}
		2217	}
		2218
		2219	changeSelectedStatus();
		2220
		2221	if (childPrim)
		2222	{
		2223	if (_parent is OdePrim)
		2224	{
		2225	OdePrim parent = (OdePrim)_parent;
		2226	parent.ChildSetGeom(this);
		2227	}
		2228	}
		2229
		2230	resetCollisionAccounting();
		2231	// m_taintshape = false;
		2232	}
		2233
		2234	/// <summary>
		2235	/// Change prim in response to an add force taint.
		2236	/// </summary>
		2237	private void changeAddForce()
		2238	{
		2239	if (!m_isSelected)
		2240	{
		2241	lock (m_forcelist)
		2242	{
		2243	//m_log.Info("[PHYSICS]: dequeing forcelist");
		2244	if (IsPhysical)
		2245	{
		2246	Vector3 iforce = Vector3.Zero;
		2247	int i = 0;
		2248	try
		2249	{
		2250	for (i = 0; i < m_forcelist.Count; i++)
		2251	{
		2252
		2253	iforce = iforce + (m_forcelist[i] * 100);
		2254	}
		2255	}
		2256	catch (IndexOutOfRangeException)
		2257	{
		2258	m_forcelist = new List<Vector3>();
		2259	m_collisionscore = 0;
		2260	m_interpenetrationcount = 0;
		2261	m_taintforce = false;
		2262	return;
		2263	}
		2264	catch (ArgumentOutOfRangeException)
		2265	{
		2266	m_forcelist = new List<Vector3>();
		2267	m_collisionscore = 0;
		2268	m_interpenetrationcount = 0;
		2269	m_taintforce = false;
		2270	return;
		2271	}
		2272	d.BodyEnable(Body);
		2273	d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
		2274	}
		2275	m_forcelist.Clear();
		2276	}
		2277
		2278	m_collisionscore = 0;
		2279	m_interpenetrationcount = 0;
		2280	}
		2281
		2282	m_taintforce = false;
		2283	}
		2284
		2285	/// <summary>
		2286	/// Change prim in response to a torque taint.
		2287	/// </summary>
		2288	private void changeSetTorque()
		2289	{
		2290	if (!m_isSelected)
		2291	{
		2292	if (IsPhysical && Body != IntPtr.Zero)
		2293	{
		2294	d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
		2295	}
		2296	}
		2297
		2298	m_taintTorque = Vector3.Zero;
		2299	}
		2300
		2301	/// <summary>
		2302	/// Change prim in response to an angular force taint.
		2303	/// </summary>
		2304	private void changeAddAngularForce()
		2305	{
		2306	if (!m_isSelected)
		2307	{
		2308	lock (m_angularforcelist)
		2309	{
		2310	//m_log.Info("[PHYSICS]: dequeing forcelist");
		2311	if (IsPhysical)
		2312	{
		2313	Vector3 iforce = Vector3.Zero;
		2314	for (int i = 0; i < m_angularforcelist.Count; i++)
		2315	{
		2316	iforce = iforce + (m_angularforcelist[i] * 100);
		2317	}
		2318	d.BodyEnable(Body);
		2319	d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
		2320
		2321	}
		2322	m_angularforcelist.Clear();
		2323	}
		2324
		2325	m_collisionscore = 0;
		2326	m_interpenetrationcount = 0;
		2327	}
		2328
		2329	m_taintaddangularforce = false;
		2330	}
		2331
		2332	/// <summary>
		2333	/// Change prim in response to a velocity taint.
		2334	/// </summary>
		2335	private void changevelocity()
		2336	{
		2337	if (!m_isSelected)
		2338	{
		2339	// Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
		2340	// walking through a default rez size prim if it keeps kicking it around - justincc.
		2341	Thread.Sleep(20);
		2342
		2343	if (IsPhysical)
		2344	{
		2345	if (Body != IntPtr.Zero)
		2346	{
		2347	d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
		2348	}
		2349	}
		2350
		2351	//resetCollisionAccounting();
		2352	}
		2353
		2354	m_taintVelocity = Vector3.Zero;
		2355	}
		2356
		2357	internal void setPrimForRemoval()
		2358	{
		2359	m_taintremove = true;
		2360	}
		2361
		2362	public override bool Flying
		2363	{
		2364	// no flying prims for you
		2365	get { return false; }
		2366	set { }
		2367	}
		2368
		2369	public override bool IsColliding
		2370	{
		2371	get { return iscolliding; }
		2372	set { iscolliding = value; }
		2373	}
		2374
		2375	public override bool CollidingGround
		2376	{
		2377	get { return false; }
		2378	set { return; }
		2379	}
		2380
		2381	public override bool CollidingObj
		2382	{
		2383	get { return false; }
		2384	set { return; }
		2385	}
		2386
		2387	public override bool ThrottleUpdates
		2388	{
		2389	get { return m_throttleUpdates; }
		2390	set { m_throttleUpdates = value; }
		2391	}
		2392
		2393	public override bool Stopped
		2394	{
		2395	get { return _zeroFlag; }
		2396	}
		2397
		2398	public override Vector3 Position
		2399	{
		2400	get { return _position; }
		2401
		2402	set { _position = value;
		2403	//m_log.Info("[PHYSICS]: " + _position.ToString());
		2404	}
		2405	}
		2406
		2407	public override Vector3 Size
		2408	{
		2409	get { return _size; }
		2410	set
		2411	{
		2412	if (value.IsFinite())
		2413	{
		2414	_size = value;
		2415	// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
		2416	}
		2417	else
		2418	{
		2419	m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
		2420	}
		2421	}
		2422	}
		2423
		2424	public override float Mass
		2425	{
		2426	get { return CalculateMass(); }
		2427	}
		2428
		2429	public override Vector3 Force
		2430	{
		2431	//get { return Vector3.Zero; }
		2432	get { return m_force; }
		2433	set
		2434	{
		2435	if (value.IsFinite())
		2436	{
		2437	m_force = value;
		2438	}
		2439	else
		2440	{
		2441	m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
		2442	}
		2443	}
		2444	}
		2445
		2446	public override int VehicleType
		2447	{
		2448	get { return (int)m_vehicle.Type; }
		2449	set { m_vehicle.ProcessTypeChange((Vehicle)value); }
		2450	}
		2451
		2452	public override void VehicleFloatParam(int param, float value)
		2453	{
		2454	m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
		2455	}
		2456
		2457	public override void VehicleVectorParam(int param, Vector3 value)
		2458	{
		2459	m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
		2460	}
		2461
		2462	public override void VehicleRotationParam(int param, Quaternion rotation)
		2463	{
		2464	m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
		2465	}
		2466
		2467	public override void VehicleFlags(int param, bool remove)
		2468	{
		2469	m_vehicle.ProcessVehicleFlags(param, remove);
		2470	}
		2471
		2472	public override void SetVolumeDetect(int param)
		2473	{
		2474	// We have to lock the scene here so that an entire simulate loop either uses volume detect for all
		2475	// possible collisions with this prim or for none of them.
		2476	lock (_parent_scene.OdeLock)
		2477	{
		2478	m_isVolumeDetect = (param != 0);
		2479	}
		2480	}
		2481
		2482	public override Vector3 CenterOfMass
		2483	{
		2484	get { return Vector3.Zero; }
		2485	}
		2486
		2487	public override Vector3 GeometricCenter
		2488	{
		2489	get { return Vector3.Zero; }
		2490	}
		2491
		2492	public override PrimitiveBaseShape Shape
		2493	{
		2494	set
		2495	{
		2496	_pbs = value;
		2497	m_assetFailed = false;
		2498	m_taintshape = true;
		2499	}
		2500	}
		2501
		2502	public override Vector3 Velocity
		2503	{
		2504	get
		2505	{
		2506	// Average previous velocity with the new one so
		2507	// client object interpolation works a 'little' better
		2508	if (_zeroFlag)
		2509	return Vector3.Zero;
		2510
		2511	Vector3 returnVelocity = Vector3.Zero;
		2512	returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
		2513	returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
		2514	returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
		2515	return returnVelocity;
		2516	}
		2517	set
		2518	{
		2519	if (value.IsFinite())
		2520	{
		2521	_velocity = value;
		2522
		2523	m_taintVelocity = value;
		2524	_parent_scene.AddPhysicsActorTaint(this);
		2525	}
		2526	else
		2527	{
		2528	m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
		2529	}
		2530
		2531	}
		2532	}
		2533
		2534	public override Vector3 Torque
		2535	{
		2536	get
		2537	{
		2538	if (!IsPhysical || Body == IntPtr.Zero)
		2539	return Vector3.Zero;
		2540
		2541	return _torque;
		2542	}
		2543
		2544	set
		2545	{
		2546	if (value.IsFinite())
		2547	{
		2548	m_taintTorque = value;
		2549	_parent_scene.AddPhysicsActorTaint(this);
		2550	}
		2551	else
		2552	{
		2553	m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
		2554	}
		2555	}
		2556	}
		2557
		2558	public override float CollisionScore
		2559	{
		2560	get { return m_collisionscore; }
		2561	set { m_collisionscore = value; }
		2562	}
		2563
		2564	public override bool Kinematic
		2565	{
		2566	get { return false; }
		2567	set { }
		2568	}
		2569
		2570	public override Quaternion Orientation
		2571	{
		2572	get { return _orientation; }
		2573	set
		2574	{
		2575	if (QuaternionIsFinite(value))
		2576	_orientation = value;
		2577	else
		2578	m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
		2579	}
		2580	}
		2581
		2582	private static bool QuaternionIsFinite(Quaternion q)
		2583	{
		2584	if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
		2585	return false;
		2586	if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
		2587	return false;
		2588	if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
		2589	return false;
		2590	if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
		2591	return false;
		2592	return true;
		2593	}
		2594
		2595	public override Vector3 Acceleration
		2596	{
		2597	get { return _acceleration; }
		2598	set { _acceleration = value; }
		2599	}
		2600
		2601	public override void AddForce(Vector3 force, bool pushforce)
		2602	{
		2603	if (force.IsFinite())
		2604	{
		2605	lock (m_forcelist)
		2606	m_forcelist.Add(force);
		2607
		2608	m_taintforce = true;
		2609	}
		2610	else
		2611	{
		2612	m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
		2613	}
		2614	//m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
		2615	}
		2616
		2617	public override void AddAngularForce(Vector3 force, bool pushforce)
		2618	{
		2619	if (force.IsFinite())
		2620	{
		2621	m_angularforcelist.Add(force);
		2622	m_taintaddangularforce = true;
		2623	}
		2624	else
		2625	{
		2626	m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
		2627	}
		2628	}
		2629
		2630	public override Vector3 RotationalVelocity
		2631	{
		2632	get
		2633	{
		2634	Vector3 pv = Vector3.Zero;
		2635	if (_zeroFlag)
		2636	return pv;
		2637	m_lastUpdateSent = false;
		2638
		2639	if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
		2640	return pv;
		2641
		2642	return m_rotationalVelocity;
		2643	}
		2644	set
		2645	{
		2646	if (value.IsFinite())
		2647	{
		2648	m_rotationalVelocity = value;
		2649	}
		2650	else
		2651	{
		2652	m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
		2653	}
		2654	}
		2655	}
		2656
		2657	public override void CrossingFailure()
		2658	{
		2659	m_crossingfailures++;
		2660	if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
		2661	{
		2662	base.RaiseOutOfBounds(_position);
		2663	return;
		2664	}
		2665	else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
		2666	{
		2667	m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
		2668	}
		2669	}
		2670
		2671	public override float Buoyancy
		2672	{
		2673	get { return m_buoyancy; }
		2674	set { m_buoyancy = value; }
		2675	}
		2676
		2677	public override void link(PhysicsActor obj)
		2678	{
		2679	m_taintparent = obj;
		2680	}
		2681
		2682	public override void delink()
		2683	{
		2684	m_taintparent = null;
		2685	}
		2686
		2687	public override void LockAngularMotion(Vector3 axis)
		2688	{
		2689	// reverse the zero/non zero values for ODE.
		2690	if (axis.IsFinite())
		2691	{
		2692	axis.X = (axis.X > 0) ? 1f : 0f;
		2693	axis.Y = (axis.Y > 0) ? 1f : 0f;
		2694	axis.Z = (axis.Z > 0) ? 1f : 0f;
		2695	m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
		2696	m_taintAngularLock = axis;
		2697	}
		2698	else
		2699	{
		2700	m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
		2701	}
		2702	}
		2703
		2704	internal void UpdatePositionAndVelocity()
		2705	{
		2706	// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
		2707	if (_parent == null)
		2708	{
		2709	Vector3 pv = Vector3.Zero;
		2710	bool lastZeroFlag = _zeroFlag;
		2711	float m_minvelocity = 0;
		2712	if (Body != (IntPtr)0) // FIXME -> or if it is a joint
		2713	{
		2714	d.Vector3 vec = d.BodyGetPosition(Body);
		2715	d.Quaternion ori = d.BodyGetQuaternion(Body);
		2716	d.Vector3 vel = d.BodyGetLinearVel(Body);
		2717	d.Vector3 rotvel = d.BodyGetAngularVel(Body);
		2718	d.Vector3 torque = d.BodyGetTorque(Body);
		2719	_torque = new Vector3(torque.X, torque.Y, torque.Z);
		2720	Vector3 l_position = Vector3.Zero;
		2721	Quaternion l_orientation = Quaternion.Identity;
		2722
		2723	// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
		2724	//if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
		2725	//if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
		2726	//if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
		2727	//if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
		2728
		2729	m_lastposition = _position;
		2730	m_lastorientation = _orientation;
		2731
		2732	l_position.X = vec.X;
		2733	l_position.Y = vec.Y;
		2734	l_position.Z = vec.Z;
		2735	l_orientation.X = ori.X;
		2736	l_orientation.Y = ori.Y;
		2737	l_orientation.Z = ori.Z;
		2738	l_orientation.W = ori.W;
		2739
		2740	if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
		2741	{
		2742	//base.RaiseOutOfBounds(l_position);
		2743
		2744	if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
		2745	{
		2746	_position = l_position;
		2747	//_parent_scene.remActivePrim(this);
		2748	if (_parent == null)
		2749	base.RequestPhysicsterseUpdate();
		2750	return;
		2751	}
		2752	else
		2753	{
		2754	if (_parent == null)
		2755	base.RaiseOutOfBounds(l_position);
		2756	return;
		2757	}
		2758	}
		2759
		2760	if (l_position.Z < 0)
		2761	{
		2762	// This is so prim that get lost underground don't fall forever and suck up
		2763	//
		2764	// Sim resources and memory.
		2765	// Disables the prim's movement physics....
		2766	// It's a hack and will generate a console message if it fails.
		2767
		2768	//IsPhysical = false;
		2769	if (_parent == null)
		2770	base.RaiseOutOfBounds(_position);
		2771
		2772	_acceleration.X = 0;
		2773	_acceleration.Y = 0;
		2774	_acceleration.Z = 0;
		2775
		2776	_velocity.X = 0;
		2777	_velocity.Y = 0;
		2778	_velocity.Z = 0;
		2779	m_rotationalVelocity.X = 0;
		2780	m_rotationalVelocity.Y = 0;
		2781	m_rotationalVelocity.Z = 0;
		2782
		2783	if (_parent == null)
		2784	base.RequestPhysicsterseUpdate();
		2785
		2786	m_throttleUpdates = false;
		2787	throttleCounter = 0;
		2788	_zeroFlag = true;
		2789	//outofBounds = true;
		2790	}
		2791
		2792	//float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
		2793	//Console.WriteLine("Adiff " + Name + " = " + Adiff);
		2794	if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
		2795	&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
		2796	&& (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
		2797	// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
		2798	&& (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
		2799	{
		2800	_zeroFlag = true;
		2801	//Console.WriteLine("ZFT 2");
		2802	m_throttleUpdates = false;
		2803	}
		2804	else
		2805	{
		2806	//m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
		2807	_zeroFlag = false;
		2808	m_lastUpdateSent = false;
		2809	//m_throttleUpdates = false;
		2810	}
		2811
		2812	if (_zeroFlag)
		2813	{
		2814	_velocity.X = 0.0f;
		2815	_velocity.Y = 0.0f;
		2816	_velocity.Z = 0.0f;
		2817
		2818	_acceleration.X = 0;
		2819	_acceleration.Y = 0;
		2820	_acceleration.Z = 0;
		2821
		2822	//_orientation.w = 0f;
		2823	//_orientation.X = 0f;
		2824	//_orientation.Y = 0f;
		2825	//_orientation.Z = 0f;
		2826	m_rotationalVelocity.X = 0;
		2827	m_rotationalVelocity.Y = 0;
		2828	m_rotationalVelocity.Z = 0;
		2829	if (!m_lastUpdateSent)
		2830	{
		2831	m_throttleUpdates = false;
		2832	throttleCounter = 0;
		2833	m_rotationalVelocity = pv;
		2834
		2835	if (_parent == null)
		2836	{
		2837	base.RequestPhysicsterseUpdate();
		2838	}
		2839
		2840	m_lastUpdateSent = true;
		2841	}
		2842	}
		2843	else
		2844	{
		2845	if (lastZeroFlag != _zeroFlag)
		2846	{
		2847	if (_parent == null)
		2848	{
		2849	base.RequestPhysicsterseUpdate();
		2850	}
		2851	}
		2852
		2853	m_lastVelocity = _velocity;
		2854
		2855	_position = l_position;
		2856
		2857	_velocity.X = vel.X;
		2858	_velocity.Y = vel.Y;
		2859	_velocity.Z = vel.Z;
		2860
		2861	_acceleration = ((_velocity - m_lastVelocity) / 0.1f);
		2862	_acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
		2863	//m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
		2864
		2865	// Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
		2866	// it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
		2867	// reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
		2868	// adding these logical exclusion situations to maintain this where I think it was intended to be.
		2869	if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
		2870	{
		2871	m_minvelocity = 0.5f;
		2872	}
		2873	else
		2874	{
		2875	m_minvelocity = 0.02f;
		2876	}
		2877
		2878	if (_velocity.ApproxEquals(pv, m_minvelocity))
		2879	{
		2880	m_rotationalVelocity = pv;
		2881	}
		2882	else
		2883	{
		2884	m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
		2885	}
		2886
		2887	//m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
		2888	_orientation.X = ori.X;
		2889	_orientation.Y = ori.Y;
		2890	_orientation.Z = ori.Z;
		2891	_orientation.W = ori.W;
		2892	m_lastUpdateSent = false;
		2893	if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
		2894	{
		2895	if (_parent == null)
		2896	{
		2897	base.RequestPhysicsterseUpdate();
		2898	}
		2899	}
		2900	else
		2901	{
		2902	throttleCounter++;
		2903	}
		2904	}
		2905	m_lastposition = l_position;
		2906	}
		2907	else
		2908	{
		2909	// Not a body.. so Make sure the client isn't interpolating
		2910	_velocity.X = 0;
		2911	_velocity.Y = 0;
		2912	_velocity.Z = 0;
		2913
		2914	_acceleration.X = 0;
		2915	_acceleration.Y = 0;
		2916	_acceleration.Z = 0;
		2917
		2918	m_rotationalVelocity.X = 0;
		2919	m_rotationalVelocity.Y = 0;
		2920	m_rotationalVelocity.Z = 0;
		2921	_zeroFlag = true;
		2922	}
		2923	}
		2924	}
		2925
		2926	public override bool FloatOnWater
		2927	{
		2928	set {
		2929	m_taintCollidesWater = value;
		2930	_parent_scene.AddPhysicsActorTaint(this);
		2931	}
		2932	}
		2933
		2934	public override void SetMomentum(Vector3 momentum)
		2935	{
		2936	}
		2937
		2938	public override Vector3 PIDTarget
		2939	{
		2940	set
		2941	{
		2942	if (value.IsFinite())
		2943	{
		2944	m_PIDTarget = value;
		2945	}
		2946	else
		2947	m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
		2948	}
		2949	}
		2950	public override bool PIDActive { set { m_usePID = value; } }
		2951	public override float PIDTau { set { m_PIDTau = value; } }
		2952
		2953	public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
		2954	public override bool PIDHoverActive { set { m_useHoverPID = value; } }
		2955	public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
		2956	public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
		2957
		2958	public override Quaternion APIDTarget{ set { return; } }
		2959
		2960	public override bool APIDActive{ set { return; } }
		2961
		2962	public override float APIDStrength{ set { return; } }
		2963
		2964	public override float APIDDamping{ set { return; } }
		2965
		2966	private void createAMotor(Vector3 axis)
		2967	{
		2968	if (Body == IntPtr.Zero)
		2969	return;
		2970
		2971	if (Amotor != IntPtr.Zero)
		2972	{
		2973	d.JointDestroy(Amotor);
		2974	Amotor = IntPtr.Zero;
		2975	}
		2976
		2977	float axisnum = 3;
		2978
		2979	axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
		2980
		2981	// PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
		2982
		2983
		2984	// Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
		2985	d.Mass objMass;
		2986	d.MassSetZero(out objMass);
		2987	DMassCopy(ref pMass, ref objMass);
		2988
		2989	//m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
		2990
		2991	Matrix4 dMassMat = FromDMass(objMass);
		2992
		2993	Matrix4 mathmat = Inverse(dMassMat);
		2994
		2995	/*
		2996	//m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
		2997
		2998	mathmat = Inverse(mathmat);
		2999
		3000
		3001	objMass = FromMatrix4(mathmat, ref objMass);
		3002	//m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
		3003
		3004	mathmat = Inverse(mathmat);
		3005	*/
		3006	if (axis.X == 0)
		3007	{
		3008	mathmat.M33 = 50.0000001f;
		3009	//objMass.I.M22 = 0;
		3010	}
		3011	if (axis.Y == 0)
		3012	{
		3013	mathmat.M22 = 50.0000001f;
		3014	//objMass.I.M11 = 0;
		3015	}
		3016	if (axis.Z == 0)
		3017	{
		3018	mathmat.M11 = 50.0000001f;
		3019	//objMass.I.M00 = 0;
		3020	}
		3021
		3022
		3023
		3024	mathmat = Inverse(mathmat);
		3025	objMass = FromMatrix4(mathmat, ref objMass);
		3026	//m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
		3027
		3028	//return;
		3029	if (d.MassCheck(ref objMass))
		3030	{
		3031	d.BodySetMass(Body, ref objMass);
		3032	}
		3033	else
		3034	{
		3035	//m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
		3036	}
		3037
		3038	if (axisnum <= 0)
		3039	return;
		3040	// int dAMotorEuler = 1;
		3041
		3042	Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
		3043	d.JointAttach(Amotor, Body, IntPtr.Zero);
		3044	d.JointSetAMotorMode(Amotor, 0);
		3045
		3046	d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
		3047	int i = 0;
		3048
		3049	if (axis.X == 0)
		3050	{
		3051	d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
		3052	i++;
		3053	}
		3054
		3055	if (axis.Y == 0)
		3056	{
		3057	d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
		3058	i++;
		3059	}
		3060
		3061	if (axis.Z == 0)
		3062	{
		3063	d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
		3064	i++;
		3065	}
		3066
		3067	for (int j = 0; j < (int)axisnum; j++)
		3068	{
		3069	//d.JointSetAMotorAngle(Amotor, j, 0);
		3070	}
		3071
		3072	//d.JointSetAMotorAngle(Amotor, 1, 0);
		3073	//d.JointSetAMotorAngle(Amotor, 2, 0);
		3074
		3075	// These lowstops and high stops are effectively (no wiggle room)
		3076	d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
		3077	d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
		3078	d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
		3079	d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
		3080	d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
		3081	d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
		3082	//d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
		3083	d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
		3084	d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
		3085	}
		3086
		3087	private Matrix4 FromDMass(d.Mass pMass)
		3088	{
		3089	Matrix4 obj;
		3090	obj.M11 = pMass.I.M00;
		3091	obj.M12 = pMass.I.M01;
		3092	obj.M13 = pMass.I.M02;
		3093	obj.M14 = 0;
		3094	obj.M21 = pMass.I.M10;
		3095	obj.M22 = pMass.I.M11;
		3096	obj.M23 = pMass.I.M12;
		3097	obj.M24 = 0;
		3098	obj.M31 = pMass.I.M20;
		3099	obj.M32 = pMass.I.M21;
		3100	obj.M33 = pMass.I.M22;
		3101	obj.M34 = 0;
		3102	obj.M41 = 0;
		3103	obj.M42 = 0;
		3104	obj.M43 = 0;
		3105	obj.M44 = 1;
		3106	return obj;
		3107	}
		3108
		3109	private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
		3110	{
		3111	obj.I.M00 = pMat[0, 0];
		3112	obj.I.M01 = pMat[0, 1];
		3113	obj.I.M02 = pMat[0, 2];
		3114	obj.I.M10 = pMat[1, 0];
		3115	obj.I.M11 = pMat[1, 1];
		3116	obj.I.M12 = pMat[1, 2];
		3117	obj.I.M20 = pMat[2, 0];
		3118	obj.I.M21 = pMat[2, 1];
		3119	obj.I.M22 = pMat[2, 2];
		3120	return obj;
		3121	}
		3122
		3123	public override void SubscribeEvents(int ms)
		3124	{
		3125	m_eventsubscription = ms;
		3126	_parent_scene.AddCollisionEventReporting(this);
		3127	}
		3128
		3129	public override void UnSubscribeEvents()
		3130	{
		3131	_parent_scene.RemoveCollisionEventReporting(this);
		3132	m_eventsubscription = 0;
		3133	}
		3134
		3135	public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
		3136	{
		3137	CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
		3138	}
		3139
		3140	public void SendCollisions()
		3141	{
		3142	if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
		3143	{
		3144	base.SendCollisionUpdate(CollisionEventsThisFrame);
		3145
		3146	if (CollisionEventsThisFrame.Count > 0)
		3147	{
		3148	m_collisionsOnPreviousFrame = true;
		3149	CollisionEventsThisFrame.Clear();
		3150	}
		3151	else
		3152	{
		3153	m_collisionsOnPreviousFrame = false;
		3154	}
		3155	}
		3156	}
		3157
		3158	public override bool SubscribedEvents()
		3159	{
		3160	if (m_eventsubscription > 0)
		3161	return true;
		3162	return false;
		3163	}
		3164
		3165	public static Matrix4 Inverse(Matrix4 pMat)
		3166	{
		3167	if (determinant3x3(pMat) == 0)
		3168	{
		3169	return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
		3170	}
		3171
		3172	return (Adjoint(pMat) / determinant3x3(pMat));
		3173	}
		3174
		3175	public static Matrix4 Adjoint(Matrix4 pMat)
		3176	{
		3177	Matrix4 adjointMatrix = new Matrix4();
		3178	for (int i=0; i<4; i++)
		3179	{
		3180	for (int j=0; j<4; j++)
		3181	{
		3182	Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
		3183	}
		3184	}
		3185
		3186	adjointMatrix = Transpose(adjointMatrix);
		3187	return adjointMatrix;
		3188	}
		3189
		3190	public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
		3191	{
		3192	Matrix4 minor = new Matrix4();
		3193	int m = 0, n = 0;
		3194	for (int i = 0; i < 4; i++)
		3195	{
		3196	if (i == iRow)
		3197	continue;
		3198	n = 0;
		3199	for (int j = 0; j < 4; j++)
		3200	{
		3201	if (j == iCol)
		3202	continue;
		3203	Matrix4SetValue(ref minor, m,n, matrix[i, j]);
		3204	n++;
		3205	}
		3206	m++;
		3207	}
		3208
		3209	return minor;
		3210	}
		3211
		3212	public static Matrix4 Transpose(Matrix4 pMat)
		3213	{
		3214	Matrix4 transposeMatrix = new Matrix4();
		3215	for (int i = 0; i < 4; i++)
		3216	for (int j = 0; j < 4; j++)
		3217	Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
		3218	return transposeMatrix;
		3219	}
		3220
		3221	public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
		3222	{
		3223	switch (r)
		3224	{
		3225	case 0:
		3226	switch (c)
		3227	{
		3228	case 0:
		3229	pMat.M11 = val;
		3230	break;
		3231	case 1:
		3232	pMat.M12 = val;
		3233	break;
		3234	case 2:
		3235	pMat.M13 = val;
		3236	break;
		3237	case 3:
		3238	pMat.M14 = val;
		3239	break;
		3240	}
		3241
		3242	break;
		3243	case 1:
		3244	switch (c)
		3245	{
		3246	case 0:
		3247	pMat.M21 = val;
		3248	break;
		3249	case 1:
		3250	pMat.M22 = val;
		3251	break;
		3252	case 2:
		3253	pMat.M23 = val;
		3254	break;
		3255	case 3:
		3256	pMat.M24 = val;
		3257	break;
		3258	}
		3259
		3260	break;
		3261	case 2:
		3262	switch (c)
		3263	{
		3264	case 0:
		3265	pMat.M31 = val;
		3266	break;
		3267	case 1:
		3268	pMat.M32 = val;
		3269	break;
		3270	case 2:
		3271	pMat.M33 = val;
		3272	break;
		3273	case 3:
		3274	pMat.M34 = val;
		3275	break;
		3276	}
		3277
		3278	break;
		3279	case 3:
		3280	switch (c)
		3281	{
		3282	case 0:
		3283	pMat.M41 = val;
		3284	break;
		3285	case 1:
		3286	pMat.M42 = val;
		3287	break;
		3288	case 2:
		3289	pMat.M43 = val;
		3290	break;
		3291	case 3:
		3292	pMat.M44 = val;
		3293	break;
		3294	}
		3295
		3296	break;
		3297	}
		3298	}
		3299
		3300	private static float determinant3x3(Matrix4 pMat)
		3301	{
		3302	float det = 0;
		3303	float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
		3304	float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
		3305	float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
		3306	float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
		3307	float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
		3308	float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
		3309
		3310	det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
		3311	return det;
		3312	}
		3313
		3314	private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
		3315	{
		3316	dst.c.W = src.c.W;
		3317	dst.c.X = src.c.X;
		3318	dst.c.Y = src.c.Y;
		3319	dst.c.Z = src.c.Z;
		3320	dst.mass = src.mass;
		3321	dst.I.M00 = src.I.M00;
		3322	dst.I.M01 = src.I.M01;
		3323	dst.I.M02 = src.I.M02;
		3324	dst.I.M10 = src.I.M10;
		3325	dst.I.M11 = src.I.M11;
		3326	dst.I.M12 = src.I.M12;
		3327	dst.I.M20 = src.I.M20;
		3328	dst.I.M21 = src.I.M21;
		3329	dst.I.M22 = src.I.M22;
		3330	}
		3331
		3332	public override void SetMaterial(int pMaterial)
		3333	{
		3334	m_material = pMaterial;
		3335	}
		3336
		3337	private void CheckMeshAsset()
		3338	{
		3339	if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
		3340	{
		3341	m_assetFailed = true;
		3342	Util.FireAndForget(delegate
		3343	{
		3344	RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
		3345	if (assetProvider != null)
		3346	assetProvider(_pbs.SculptTexture, MeshAssetReceived);
		3347	});
		3348	}
		3349	}
		3350
		3351	private void MeshAssetReceived(AssetBase asset)
		3352	{
		3353	if (asset != null && asset.Data != null && asset.Data.Length > 0)
		3354	{
		3355	if (!_pbs.SculptEntry)
		3356	return;
		3357	if (_pbs.SculptTexture.ToString() != asset.ID)
		3358	return;
		3359
		3360	_pbs.SculptData = new byte[asset.Data.Length];
		3361	asset.Data.CopyTo(_pbs.SculptData, 0);
		3362	// m_assetFailed = false;
		3363
		3364	// m_log.DebugFormat(
		3365	// "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
		3366	// _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
		3367
		3368	m_taintshape = true;
		3369	_parent_scene.AddPhysicsActorTaint(this);
		3370	}
		3371	else
		3372	{
		3373	m_log.WarnFormat(
		3374	"[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
		3375	_pbs.SculptTexture, Name, _position, _parent_scene.Name);
		3376	}
		3377	}
		3378	}
		3379	}