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

clockwerk-opensim – Blame information for rev 1

Rev	Author	Line No.	Line
1	vero	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	* The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
		28	* are Copyright (c) 2009 Linden Research, Inc and are used under their license
		29	* of Creative Commons Attribution-Share Alike 3.0
		30	* (http://creativecommons.org/licenses/by-sa/3.0/).
		31	*/
		32
		33	using System;
		34	using System.Collections.Generic;
		35	using System.Reflection;
		36	using System.Runtime.InteropServices;
		37	using OpenMetaverse;
		38	using OpenSim.Framework;
		39	using OpenSim.Region.Physics.Manager;
		40
		41	namespace OpenSim.Region.Physics.BulletSPlugin
		42	{
		43	public sealed class BSDynamics : BSActor
		44	{
		45	#pragma warning disable 414
		46	private static string LogHeader = "[BULLETSIM VEHICLE]";
		47	#pragma warning restore 414
		48
		49	// the prim this dynamic controller belongs to
		50	private BSPrimLinkable ControllingPrim { get; set; }
		51
		52	private bool m_haveRegisteredForSceneEvents;
		53
		54	// mass of the vehicle fetched each time we're calles
		55	private float m_vehicleMass;
		56
		57	// Vehicle properties
		58	public Vehicle Type { get; set; }
		59
		60	// private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
		61	private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
		62	// HOVER_TERRAIN_ONLY
		63	// HOVER_GLOBAL_HEIGHT
		64	// NO_DEFLECTION_UP
		65	// HOVER_WATER_ONLY
		66	// HOVER_UP_ONLY
		67	// LIMIT_MOTOR_UP
		68	// LIMIT_ROLL_ONLY
		69	private Vector3 m_BlockingEndPoint = Vector3.Zero;
		70	private Quaternion m_RollreferenceFrame = Quaternion.Identity;
		71	private Quaternion m_referenceFrame = Quaternion.Identity;
		72
		73	// Linear properties
		74	private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
		75	private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
		76	private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
		77	private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
		78	private Vector3 m_linearFrictionTimescale = Vector3.Zero;
		79	private float m_linearMotorDecayTimescale = 0;
		80	private float m_linearMotorTimescale = 0;
		81	private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
		82	private Vector3 m_lastPositionVector = Vector3.Zero;
		83	// private bool m_LinearMotorSetLastFrame = false;
		84	// private Vector3 m_linearMotorOffset = Vector3.Zero;
		85
		86	//Angular properties
		87	private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
		88	private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
		89	// private int m_angularMotorApply = 0; // application frame counter
		90	private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
		91	private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
		92	private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
		93	private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
		94	private Vector3 m_lastAngularVelocity = Vector3.Zero;
		95	private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
		96
		97	//Deflection properties
		98	private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
		99	private float m_angularDeflectionEfficiency = 0;
		100	private float m_angularDeflectionTimescale = 0;
		101	private float m_linearDeflectionEfficiency = 0;
		102	private float m_linearDeflectionTimescale = 0;
		103
		104	//Banking properties
		105	private float m_bankingEfficiency = 0;
		106	private float m_bankingMix = 0;
		107	private float m_bankingTimescale = 0;
		108
		109	//Hover and Buoyancy properties
		110	private BSVMotor m_hoverMotor = new BSVMotor("Hover");
		111	private float m_VhoverHeight = 0f;
		112	private float m_VhoverEfficiency = 0f;
		113	private float m_VhoverTimescale = 0f;
		114	private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
		115	// Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
		116	private float m_VehicleBuoyancy = 0f;
		117	private Vector3 m_VehicleGravity = Vector3.Zero; // Gravity computed when buoyancy set
		118
		119	//Attractor properties
		120	private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
		121	private float m_verticalAttractionEfficiency = 1.0f; // damped
		122	private float m_verticalAttractionCutoff = 500f; // per the documentation
		123	// Timescale > cutoff means no vert attractor.
		124	private float m_verticalAttractionTimescale = 510f;
		125
		126	// Just some recomputed constants:
		127	static readonly float PIOverFour = ((float)Math.PI) / 4f;
		128	#pragma warning disable 414
		129	static readonly float PIOverTwo = ((float)Math.PI) / 2f;
		130	#pragma warning restore 414
		131
		132	public BSDynamics(BSScene myScene, BSPrim myPrim, string actorName)
		133	: base(myScene, myPrim, actorName)
		134	{
		135	Type = Vehicle.TYPE_NONE;
		136	m_haveRegisteredForSceneEvents = false;
		137
		138	ControllingPrim = myPrim as BSPrimLinkable;
		139	if (ControllingPrim == null)
		140	{
		141	// THIS CANNOT HAPPEN!!
		142	}
		143	VDetailLog("{0},Creation", ControllingPrim.LocalID);
		144	}
		145
		146	// Return 'true' if this vehicle is doing vehicle things
		147	public bool IsActive
		148	{
		149	get { return (Type != Vehicle.TYPE_NONE && ControllingPrim.IsPhysicallyActive); }
		150	}
		151
		152	// Return 'true' if this a vehicle that should be sitting on the ground
		153	public bool IsGroundVehicle
		154	{
		155	get { return (Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED); }
		156	}
		157
		158	#region Vehicle parameter setting
		159	public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
		160	{
		161	VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
		162	switch (pParam)
		163	{
		164	case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
		165	m_angularDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
		166	break;
		167	case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
		168	m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
		169	break;
		170	case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
		171	m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
		172	m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
		173	break;
		174	case Vehicle.ANGULAR_MOTOR_TIMESCALE:
		175	m_angularMotorTimescale = Math.Max(pValue, 0.01f);
		176	m_angularMotor.TimeScale = m_angularMotorTimescale;
		177	break;
		178	case Vehicle.BANKING_EFFICIENCY:
		179	m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
		180	break;
		181	case Vehicle.BANKING_MIX:
		182	m_bankingMix = Math.Max(pValue, 0.01f);
		183	break;
		184	case Vehicle.BANKING_TIMESCALE:
		185	m_bankingTimescale = Math.Max(pValue, 0.01f);
		186	break;
		187	case Vehicle.BUOYANCY:
		188	m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
		189	m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
		190	break;
		191	case Vehicle.HOVER_EFFICIENCY:
		192	m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
		193	break;
		194	case Vehicle.HOVER_HEIGHT:
		195	m_VhoverHeight = pValue;
		196	break;
		197	case Vehicle.HOVER_TIMESCALE:
		198	m_VhoverTimescale = Math.Max(pValue, 0.01f);
		199	break;
		200	case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
		201	m_linearDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
		202	break;
		203	case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
		204	m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
		205	break;
		206	case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
		207	m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
		208	m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
		209	break;
		210	case Vehicle.LINEAR_MOTOR_TIMESCALE:
		211	m_linearMotorTimescale = Math.Max(pValue, 0.01f);
		212	m_linearMotor.TimeScale = m_linearMotorTimescale;
		213	break;
		214	case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
		215	m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
		216	m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
		217	break;
		218	case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
		219	m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
		220	m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
		221	break;
		222
		223	// These are vector properties but the engine lets you use a single float value to
		224	// set all of the components to the same value
		225	case Vehicle.ANGULAR_FRICTION_TIMESCALE:
		226	m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
		227	break;
		228	case Vehicle.ANGULAR_MOTOR_DIRECTION:
		229	m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
		230	m_angularMotor.Zero();
		231	m_angularMotor.SetTarget(m_angularMotorDirection);
		232	break;
		233	case Vehicle.LINEAR_FRICTION_TIMESCALE:
		234	m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
		235	break;
		236	case Vehicle.LINEAR_MOTOR_DIRECTION:
		237	m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
		238	m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
		239	m_linearMotor.SetTarget(m_linearMotorDirection);
		240	break;
		241	case Vehicle.LINEAR_MOTOR_OFFSET:
		242	m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
		243	break;
		244
		245	}
		246	}//end ProcessFloatVehicleParam
		247
		248	internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
		249	{
		250	VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
		251	switch (pParam)
		252	{
		253	case Vehicle.ANGULAR_FRICTION_TIMESCALE:
		254	m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
		255	break;
		256	case Vehicle.ANGULAR_MOTOR_DIRECTION:
		257	// Limit requested angular speed to 2 rps= 4 pi rads/sec
		258	pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
		259	pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
		260	pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
		261	m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
		262	m_angularMotor.Zero();
		263	m_angularMotor.SetTarget(m_angularMotorDirection);
		264	break;
		265	case Vehicle.LINEAR_FRICTION_TIMESCALE:
		266	m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
		267	break;
		268	case Vehicle.LINEAR_MOTOR_DIRECTION:
		269	m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
		270	m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
		271	m_linearMotor.SetTarget(m_linearMotorDirection);
		272	break;
		273	case Vehicle.LINEAR_MOTOR_OFFSET:
		274	m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
		275	break;
		276	case Vehicle.BLOCK_EXIT:
		277	m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
		278	break;
		279	}
		280	}//end ProcessVectorVehicleParam
		281
		282	internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
		283	{
		284	VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
		285	switch (pParam)
		286	{
		287	case Vehicle.REFERENCE_FRAME:
		288	m_referenceFrame = pValue;
		289	break;
		290	case Vehicle.ROLL_FRAME:
		291	m_RollreferenceFrame = pValue;
		292	break;
		293	}
		294	}//end ProcessRotationVehicleParam
		295
		296	internal void ProcessVehicleFlags(int pParam, bool remove)
		297	{
		298	VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", ControllingPrim.LocalID, pParam, remove);
		299	VehicleFlag parm = (VehicleFlag)pParam;
		300	if (pParam == -1)
		301	m_flags = (VehicleFlag)0;
		302	else
		303	{
		304	if (remove)
		305	m_flags &= ~parm;
		306	else
		307	m_flags |= parm;
		308	}
		309	}
		310
		311	public void ProcessTypeChange(Vehicle pType)
		312	{
		313	VDetailLog("{0},ProcessTypeChange,type={1}", ControllingPrim.LocalID, pType);
		314	// Set Defaults For Type
		315	Type = pType;
		316	switch (pType)
		317	{
		318	case Vehicle.TYPE_NONE:
		319	m_linearMotorDirection = Vector3.Zero;
		320	m_linearMotorTimescale = 0;
		321	m_linearMotorDecayTimescale = 0;
		322	m_linearFrictionTimescale = new Vector3(0, 0, 0);
		323
		324	m_angularMotorDirection = Vector3.Zero;
		325	m_angularMotorDecayTimescale = 0;
		326	m_angularMotorTimescale = 0;
		327	m_angularFrictionTimescale = new Vector3(0, 0, 0);
		328
		329	m_VhoverHeight = 0;
		330	m_VhoverEfficiency = 0;
		331	m_VhoverTimescale = 0;
		332	m_VehicleBuoyancy = 0;
		333
		334	m_linearDeflectionEfficiency = 1;
		335	m_linearDeflectionTimescale = 1;
		336
		337	m_angularDeflectionEfficiency = 0;
		338	m_angularDeflectionTimescale = 1000;
		339
		340	m_verticalAttractionEfficiency = 0;
		341	m_verticalAttractionTimescale = 0;
		342
		343	m_bankingEfficiency = 0;
		344	m_bankingTimescale = 1000;
		345	m_bankingMix = 1;
		346
		347	m_referenceFrame = Quaternion.Identity;
		348	m_flags = (VehicleFlag)0;
		349
		350	break;
		351
		352	case Vehicle.TYPE_SLED:
		353	m_linearMotorDirection = Vector3.Zero;
		354	m_linearMotorTimescale = 1000;
		355	m_linearMotorDecayTimescale = 120;
		356	m_linearFrictionTimescale = new Vector3(30, 1, 1000);
		357
		358	m_angularMotorDirection = Vector3.Zero;
		359	m_angularMotorTimescale = 1000;
		360	m_angularMotorDecayTimescale = 120;
		361	m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
		362
		363	m_VhoverHeight = 0;
		364	m_VhoverEfficiency = 10; // TODO: this looks wrong!!
		365	m_VhoverTimescale = 10;
		366	m_VehicleBuoyancy = 0;
		367
		368	m_linearDeflectionEfficiency = 1;
		369	m_linearDeflectionTimescale = 1;
		370
		371	m_angularDeflectionEfficiency = 1;
		372	m_angularDeflectionTimescale = 1000;
		373
		374	m_verticalAttractionEfficiency = 0;
		375	m_verticalAttractionTimescale = 0;
		376
		377	m_bankingEfficiency = 0;
		378	m_bankingTimescale = 10;
		379	m_bankingMix = 1;
		380
		381	m_referenceFrame = Quaternion.Identity;
		382	m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
		383	| VehicleFlag.HOVER_TERRAIN_ONLY
		384	| VehicleFlag.HOVER_GLOBAL_HEIGHT
		385	| VehicleFlag.HOVER_UP_ONLY);
		386	m_flags |= (VehicleFlag.NO_DEFLECTION_UP
		387	| VehicleFlag.LIMIT_ROLL_ONLY
		388	| VehicleFlag.LIMIT_MOTOR_UP);
		389
		390	break;
		391	case Vehicle.TYPE_CAR:
		392	m_linearMotorDirection = Vector3.Zero;
		393	m_linearMotorTimescale = 1;
		394	m_linearMotorDecayTimescale = 60;
		395	m_linearFrictionTimescale = new Vector3(100, 2, 1000);
		396
		397	m_angularMotorDirection = Vector3.Zero;
		398	m_angularMotorTimescale = 1;
		399	m_angularMotorDecayTimescale = 0.8f;
		400	m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
		401
		402	m_VhoverHeight = 0;
		403	m_VhoverEfficiency = 0;
		404	m_VhoverTimescale = 1000;
		405	m_VehicleBuoyancy = 0;
		406
		407	m_linearDeflectionEfficiency = 1;
		408	m_linearDeflectionTimescale = 2;
		409
		410	m_angularDeflectionEfficiency = 0;
		411	m_angularDeflectionTimescale = 10;
		412
		413	m_verticalAttractionEfficiency = 1f;
		414	m_verticalAttractionTimescale = 10f;
		415
		416	m_bankingEfficiency = -0.2f;
		417	m_bankingMix = 1;
		418	m_bankingTimescale = 1;
		419
		420	m_referenceFrame = Quaternion.Identity;
		421	m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
		422	| VehicleFlag.HOVER_TERRAIN_ONLY
		423	| VehicleFlag.HOVER_GLOBAL_HEIGHT);
		424	m_flags |= (VehicleFlag.NO_DEFLECTION_UP
		425	| VehicleFlag.LIMIT_ROLL_ONLY
		426	| VehicleFlag.LIMIT_MOTOR_UP
		427	| VehicleFlag.HOVER_UP_ONLY);
		428	break;
		429	case Vehicle.TYPE_BOAT:
		430	m_linearMotorDirection = Vector3.Zero;
		431	m_linearMotorTimescale = 5;
		432	m_linearMotorDecayTimescale = 60;
		433	m_linearFrictionTimescale = new Vector3(10, 3, 2);
		434
		435	m_angularMotorDirection = Vector3.Zero;
		436	m_angularMotorTimescale = 4;
		437	m_angularMotorDecayTimescale = 4;
		438	m_angularFrictionTimescale = new Vector3(10,10,10);
		439
		440	m_VhoverHeight = 0;
		441	m_VhoverEfficiency = 0.5f;
		442	m_VhoverTimescale = 2;
		443	m_VehicleBuoyancy = 1;
		444
		445	m_linearDeflectionEfficiency = 0.5f;
		446	m_linearDeflectionTimescale = 3;
		447
		448	m_angularDeflectionEfficiency = 0.5f;
		449	m_angularDeflectionTimescale = 5;
		450
		451	m_verticalAttractionEfficiency = 0.5f;
		452	m_verticalAttractionTimescale = 5f;
		453
		454	m_bankingEfficiency = -0.3f;
		455	m_bankingMix = 0.8f;
		456	m_bankingTimescale = 1;
		457
		458	m_referenceFrame = Quaternion.Identity;
		459	m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
		460	| VehicleFlag.HOVER_GLOBAL_HEIGHT
		461	| VehicleFlag.LIMIT_ROLL_ONLY
		462	| VehicleFlag.HOVER_UP_ONLY);
		463	m_flags |= (VehicleFlag.NO_DEFLECTION_UP
		464	| VehicleFlag.LIMIT_MOTOR_UP
		465	| VehicleFlag.HOVER_WATER_ONLY);
		466	break;
		467	case Vehicle.TYPE_AIRPLANE:
		468	m_linearMotorDirection = Vector3.Zero;
		469	m_linearMotorTimescale = 2;
		470	m_linearMotorDecayTimescale = 60;
		471	m_linearFrictionTimescale = new Vector3(200, 10, 5);
		472
		473	m_angularMotorDirection = Vector3.Zero;
		474	m_angularMotorTimescale = 4;
		475	m_angularMotorDecayTimescale = 4;
		476	m_angularFrictionTimescale = new Vector3(20, 20, 20);
		477
		478	m_VhoverHeight = 0;
		479	m_VhoverEfficiency = 0.5f;
		480	m_VhoverTimescale = 1000;
		481	m_VehicleBuoyancy = 0;
		482
		483	m_linearDeflectionEfficiency = 0.5f;
		484	m_linearDeflectionTimescale = 3;
		485
		486	m_angularDeflectionEfficiency = 1;
		487	m_angularDeflectionTimescale = 2;
		488
		489	m_verticalAttractionEfficiency = 0.9f;
		490	m_verticalAttractionTimescale = 2f;
		491
		492	m_bankingEfficiency = 1;
		493	m_bankingMix = 0.7f;
		494	m_bankingTimescale = 2;
		495
		496	m_referenceFrame = Quaternion.Identity;
		497	m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
		498	| VehicleFlag.HOVER_TERRAIN_ONLY
		499	| VehicleFlag.HOVER_GLOBAL_HEIGHT
		500	| VehicleFlag.HOVER_UP_ONLY
		501	| VehicleFlag.NO_DEFLECTION_UP
		502	| VehicleFlag.LIMIT_MOTOR_UP);
		503	m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
		504	break;
		505	case Vehicle.TYPE_BALLOON:
		506	m_linearMotorDirection = Vector3.Zero;
		507	m_linearMotorTimescale = 5;
		508	m_linearFrictionTimescale = new Vector3(5, 5, 5);
		509	m_linearMotorDecayTimescale = 60;
		510
		511	m_angularMotorDirection = Vector3.Zero;
		512	m_angularMotorTimescale = 6;
		513	m_angularFrictionTimescale = new Vector3(10, 10, 10);
		514	m_angularMotorDecayTimescale = 10;
		515
		516	m_VhoverHeight = 5;
		517	m_VhoverEfficiency = 0.8f;
		518	m_VhoverTimescale = 10;
		519	m_VehicleBuoyancy = 1;
		520
		521	m_linearDeflectionEfficiency = 0;
		522	m_linearDeflectionTimescale = 5;
		523
		524	m_angularDeflectionEfficiency = 0;
		525	m_angularDeflectionTimescale = 5;
		526
		527	m_verticalAttractionEfficiency = 1f;
		528	m_verticalAttractionTimescale = 100f;
		529
		530	m_bankingEfficiency = 0;
		531	m_bankingMix = 0.7f;
		532	m_bankingTimescale = 5;
		533
		534	m_referenceFrame = Quaternion.Identity;
		535
		536	m_referenceFrame = Quaternion.Identity;
		537	m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
		538	| VehicleFlag.HOVER_TERRAIN_ONLY
		539	| VehicleFlag.HOVER_UP_ONLY
		540	| VehicleFlag.NO_DEFLECTION_UP
		541	| VehicleFlag.LIMIT_MOTOR_UP);
		542	m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
		543	| VehicleFlag.HOVER_GLOBAL_HEIGHT);
		544	break;
		545	}
		546
		547	m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, m_linearMotorDecayTimescale, 1f);
		548	// m_linearMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
		549
		550	m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale, m_angularMotorDecayTimescale, 1f);
		551	// m_angularMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
		552
		553	/* Not implemented
		554	m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
		555	BSMotor.Infinite, BSMotor.InfiniteVector,
		556	m_verticalAttractionEfficiency);
		557	// Z goes away and we keep X and Y
		558	m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
		559	*/
		560
		561	if (this.Type == Vehicle.TYPE_NONE)
		562	{
		563	UnregisterForSceneEvents();
		564	}
		565	else
		566	{
		567	RegisterForSceneEvents();
		568	}
		569
		570	// Update any physical parameters based on this type.
		571	Refresh();
		572	}
		573	#endregion // Vehicle parameter setting
		574
		575	// BSActor.Refresh()
		576	public override void Refresh()
		577	{
		578	// If asking for a refresh, reset the physical parameters before the next simulation step.
		579	// Called whether active or not since the active state may be updated before the next step.
		580	m_physicsScene.PostTaintObject("BSDynamics.Refresh", ControllingPrim.LocalID, delegate()
		581	{
		582	SetPhysicalParameters();
		583	});
		584	}
		585
		586	// Some of the properties of this prim may have changed.
		587	// Do any updating needed for a vehicle
		588	private void SetPhysicalParameters()
		589	{
		590	if (IsActive)
		591	{
		592	// Remember the mass so we don't have to fetch it every step
		593	m_vehicleMass = ControllingPrim.TotalMass;
		594
		595	// Friction affects are handled by this vehicle code
		596	// m_physicsScene.PE.SetFriction(ControllingPrim.PhysBody, BSParam.VehicleFriction);
		597	// m_physicsScene.PE.SetRestitution(ControllingPrim.PhysBody, BSParam.VehicleRestitution);
		598	ControllingPrim.Linkset.SetPhysicalFriction(BSParam.VehicleFriction);
		599	ControllingPrim.Linkset.SetPhysicalRestitution(BSParam.VehicleRestitution);
		600
		601	// Moderate angular movement introduced by Bullet.
		602	// TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
		603	// Maybe compute linear and angular factor and damping from params.
		604	m_physicsScene.PE.SetAngularDamping(ControllingPrim.PhysBody, BSParam.VehicleAngularDamping);
		605	m_physicsScene.PE.SetLinearFactor(ControllingPrim.PhysBody, BSParam.VehicleLinearFactor);
		606	m_physicsScene.PE.SetAngularFactorV(ControllingPrim.PhysBody, BSParam.VehicleAngularFactor);
		607
		608	// Vehicles report collision events so we know when it's on the ground
		609	// m_physicsScene.PE.AddToCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
		610	ControllingPrim.Linkset.AddToPhysicalCollisionFlags(CollisionFlags.BS_VEHICLE_COLLISIONS);
		611
		612	// Vector3 inertia = m_physicsScene.PE.CalculateLocalInertia(ControllingPrim.PhysShape.physShapeInfo, m_vehicleMass);
		613	// ControllingPrim.Inertia = inertia * BSParam.VehicleInertiaFactor;
		614	// m_physicsScene.PE.SetMassProps(ControllingPrim.PhysBody, m_vehicleMass, ControllingPrim.Inertia);
		615	// m_physicsScene.PE.UpdateInertiaTensor(ControllingPrim.PhysBody);
		616	ControllingPrim.Linkset.ComputeAndSetLocalInertia(BSParam.VehicleInertiaFactor, m_vehicleMass);
		617
		618	// Set the gravity for the vehicle depending on the buoyancy
		619	// TODO: what should be done if prim and vehicle buoyancy differ?
		620	m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
		621	// The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
		622	// m_physicsScene.PE.SetGravity(ControllingPrim.PhysBody, Vector3.Zero);
		623	ControllingPrim.Linkset.SetPhysicalGravity(Vector3.Zero);
		624
		625	VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
		626	ControllingPrim.LocalID, m_vehicleMass, ControllingPrim.Inertia, m_VehicleGravity,
		627	BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
		628	BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
		629	);
		630	}
		631	else
		632	{
		633	if (ControllingPrim.PhysBody.HasPhysicalBody)
		634	m_physicsScene.PE.RemoveFromCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
		635	// ControllingPrim.Linkset.RemoveFromPhysicalCollisionFlags(CollisionFlags.BS_VEHICLE_COLLISIONS);
		636	}
		637	}
		638
		639	// BSActor.RemoveBodyDependencies
		640	public override void RemoveDependencies()
		641	{
		642	Refresh();
		643	}
		644
		645	// BSActor.Release()
		646	public override void Dispose()
		647	{
		648	VDetailLog("{0},Dispose", ControllingPrim.LocalID);
		649	UnregisterForSceneEvents();
		650	Type = Vehicle.TYPE_NONE;
		651	Enabled = false;
		652	return;
		653	}
		654
		655	private void RegisterForSceneEvents()
		656	{
		657	if (!m_haveRegisteredForSceneEvents)
		658	{
		659	m_physicsScene.BeforeStep += this.Step;
		660	m_physicsScene.AfterStep += this.PostStep;
		661	ControllingPrim.OnPreUpdateProperty += this.PreUpdateProperty;
		662	m_haveRegisteredForSceneEvents = true;
		663	}
		664	}
		665
		666	private void UnregisterForSceneEvents()
		667	{
		668	if (m_haveRegisteredForSceneEvents)
		669	{
		670	m_physicsScene.BeforeStep -= this.Step;
		671	m_physicsScene.AfterStep -= this.PostStep;
		672	ControllingPrim.OnPreUpdateProperty -= this.PreUpdateProperty;
		673	m_haveRegisteredForSceneEvents = false;
		674	}
		675	}
		676
		677	private void PreUpdateProperty(ref EntityProperties entprop)
		678	{
		679	// A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
		680	// TODO: handle physics introduced by Bullet with computed vehicle physics.
		681	if (IsActive)
		682	{
		683	entprop.RotationalVelocity = Vector3.Zero;
		684	}
		685	}
		686
		687	#region Known vehicle value functions
		688	// Vehicle physical parameters that we buffer from constant getting and setting.
		689	// The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
		690	// Changing is remembered and the parameter is stored back into the physics engine only if updated.
		691	// This does two things: 1) saves continuious calls into unmanaged code, and
		692	// 2) signals when a physics property update must happen back to the simulator
		693	// to update values modified for the vehicle.
		694	private int m_knownChanged;
		695	private int m_knownHas;
		696	private float m_knownTerrainHeight;
		697	private float m_knownWaterLevel;
		698	private Vector3 m_knownPosition;
		699	private Vector3 m_knownVelocity;
		700	private Vector3 m_knownForce;
		701	private Vector3 m_knownForceImpulse;
		702	private Quaternion m_knownOrientation;
		703	private Vector3 m_knownRotationalVelocity;
		704	private Vector3 m_knownRotationalForce;
		705	private Vector3 m_knownRotationalImpulse;
		706
		707	private const int m_knownChangedPosition = 1 << 0;
		708	private const int m_knownChangedVelocity = 1 << 1;
		709	private const int m_knownChangedForce = 1 << 2;
		710	private const int m_knownChangedForceImpulse = 1 << 3;
		711	private const int m_knownChangedOrientation = 1 << 4;
		712	private const int m_knownChangedRotationalVelocity = 1 << 5;
		713	private const int m_knownChangedRotationalForce = 1 << 6;
		714	private const int m_knownChangedRotationalImpulse = 1 << 7;
		715	private const int m_knownChangedTerrainHeight = 1 << 8;
		716	private const int m_knownChangedWaterLevel = 1 << 9;
		717
		718	public void ForgetKnownVehicleProperties()
		719	{
		720	m_knownHas = 0;
		721	m_knownChanged = 0;
		722	}
		723	// Push all the changed values back into the physics engine
		724	public void PushKnownChanged()
		725	{
		726	if (m_knownChanged != 0)
		727	{
		728	if ((m_knownChanged & m_knownChangedPosition) != 0)
		729	ControllingPrim.ForcePosition = m_knownPosition;
		730
		731	if ((m_knownChanged & m_knownChangedOrientation) != 0)
		732	ControllingPrim.ForceOrientation = m_knownOrientation;
		733
		734	if ((m_knownChanged & m_knownChangedVelocity) != 0)
		735	{
		736	ControllingPrim.ForceVelocity = m_knownVelocity;
		737	// Fake out Bullet by making it think the velocity is the same as last time.
		738	// Bullet does a bunch of smoothing for changing parameters.
		739	// Since the vehicle is demanding this setting, we override Bullet's smoothing
		740	// by telling Bullet the value was the same last time.
		741	// PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
		742	}
		743
		744	if ((m_knownChanged & m_knownChangedForce) != 0)
		745	ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
		746
		747	if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
		748	ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
		749
		750	if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
		751	{
		752	ControllingPrim.ForceRotationalVelocity = m_knownRotationalVelocity;
		753	// PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
		754	}
		755
		756	if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
		757	ControllingPrim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/);
		758
		759	if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
		760	{
		761	ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
		762	}
		763
		764	// If we set one of the values (ie, the physics engine didn't do it) we must force
		765	// an UpdateProperties event to send the changes up to the simulator.
		766	m_physicsScene.PE.PushUpdate(ControllingPrim.PhysBody);
		767	}
		768	m_knownChanged = 0;
		769	}
		770
		771	// Since the computation of terrain height can be a little involved, this routine
		772	// is used to fetch the height only once for each vehicle simulation step.
		773	Vector3 lastRememberedHeightPos = new Vector3(-1, -1, -1);
		774	private float GetTerrainHeight(Vector3 pos)
		775	{
		776	if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
		777	{
		778	lastRememberedHeightPos = pos;
		779	m_knownTerrainHeight = ControllingPrim.PhysScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
		780	m_knownHas |= m_knownChangedTerrainHeight;
		781	}
		782	return m_knownTerrainHeight;
		783	}
		784
		785	// Since the computation of water level can be a little involved, this routine
		786	// is used ot fetch the level only once for each vehicle simulation step.
		787	Vector3 lastRememberedWaterHeightPos = new Vector3(-1, -1, -1);
		788	private float GetWaterLevel(Vector3 pos)
		789	{
		790	if ((m_knownHas & m_knownChangedWaterLevel) == 0 || pos != lastRememberedWaterHeightPos)
		791	{
		792	lastRememberedWaterHeightPos = pos;
		793	m_knownWaterLevel = ControllingPrim.PhysScene.TerrainManager.GetWaterLevelAtXYZ(pos);
		794	m_knownHas |= m_knownChangedWaterLevel;
		795	}
		796	return m_knownWaterLevel;
		797	}
		798
		799	private Vector3 VehiclePosition
		800	{
		801	get
		802	{
		803	if ((m_knownHas & m_knownChangedPosition) == 0)
		804	{
		805	m_knownPosition = ControllingPrim.ForcePosition;
		806	m_knownHas |= m_knownChangedPosition;
		807	}
		808	return m_knownPosition;
		809	}
		810	set
		811	{
		812	m_knownPosition = value;
		813	m_knownChanged |= m_knownChangedPosition;
		814	m_knownHas |= m_knownChangedPosition;
		815	}
		816	}
		817
		818	private Quaternion VehicleOrientation
		819	{
		820	get
		821	{
		822	if ((m_knownHas & m_knownChangedOrientation) == 0)
		823	{
		824	m_knownOrientation = ControllingPrim.ForceOrientation;
		825	m_knownHas |= m_knownChangedOrientation;
		826	}
		827	return m_knownOrientation;
		828	}
		829	set
		830	{
		831	m_knownOrientation = value;
		832	m_knownChanged |= m_knownChangedOrientation;
		833	m_knownHas |= m_knownChangedOrientation;
		834	}
		835	}
		836
		837	private Vector3 VehicleVelocity
		838	{
		839	get
		840	{
		841	if ((m_knownHas & m_knownChangedVelocity) == 0)
		842	{
		843	m_knownVelocity = ControllingPrim.ForceVelocity;
		844	m_knownHas |= m_knownChangedVelocity;
		845	}
		846	return m_knownVelocity;
		847	}
		848	set
		849	{
		850	m_knownVelocity = value;
		851	m_knownChanged |= m_knownChangedVelocity;
		852	m_knownHas |= m_knownChangedVelocity;
		853	}
		854	}
		855
		856	private void VehicleAddForce(Vector3 pForce)
		857	{
		858	if ((m_knownHas & m_knownChangedForce) == 0)
		859	{
		860	m_knownForce = Vector3.Zero;
		861	m_knownHas |= m_knownChangedForce;
		862	}
		863	m_knownForce += pForce;
		864	m_knownChanged |= m_knownChangedForce;
		865	}
		866
		867	private void VehicleAddForceImpulse(Vector3 pImpulse)
		868	{
		869	if ((m_knownHas & m_knownChangedForceImpulse) == 0)
		870	{
		871	m_knownForceImpulse = Vector3.Zero;
		872	m_knownHas |= m_knownChangedForceImpulse;
		873	}
		874	m_knownForceImpulse += pImpulse;
		875	m_knownChanged |= m_knownChangedForceImpulse;
		876	}
		877
		878	private Vector3 VehicleRotationalVelocity
		879	{
		880	get
		881	{
		882	if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
		883	{
		884	m_knownRotationalVelocity = ControllingPrim.ForceRotationalVelocity;
		885	m_knownHas |= m_knownChangedRotationalVelocity;
		886	}
		887	return (Vector3)m_knownRotationalVelocity;
		888	}
		889	set
		890	{
		891	m_knownRotationalVelocity = value;
		892	m_knownChanged |= m_knownChangedRotationalVelocity;
		893	m_knownHas |= m_knownChangedRotationalVelocity;
		894	}
		895	}
		896	private void VehicleAddAngularForce(Vector3 aForce)
		897	{
		898	if ((m_knownHas & m_knownChangedRotationalForce) == 0)
		899	{
		900	m_knownRotationalForce = Vector3.Zero;
		901	}
		902	m_knownRotationalForce += aForce;
		903	m_knownChanged |= m_knownChangedRotationalForce;
		904	m_knownHas |= m_knownChangedRotationalForce;
		905	}
		906	private void VehicleAddRotationalImpulse(Vector3 pImpulse)
		907	{
		908	if ((m_knownHas & m_knownChangedRotationalImpulse) == 0)
		909	{
		910	m_knownRotationalImpulse = Vector3.Zero;
		911	m_knownHas |= m_knownChangedRotationalImpulse;
		912	}
		913	m_knownRotationalImpulse += pImpulse;
		914	m_knownChanged |= m_knownChangedRotationalImpulse;
		915	}
		916
		917	// Vehicle relative forward velocity
		918	private Vector3 VehicleForwardVelocity
		919	{
		920	get
		921	{
		922	return VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
		923	}
		924	}
		925
		926	private float VehicleForwardSpeed
		927	{
		928	get
		929	{
		930	return VehicleForwardVelocity.X;
		931	}
		932	}
		933
		934	#endregion // Known vehicle value functions
		935
		936	// One step of the vehicle properties for the next 'pTimestep' seconds.
		937	internal void Step(float pTimestep)
		938	{
		939	if (!IsActive) return;
		940
		941	ForgetKnownVehicleProperties();
		942
		943	MoveLinear(pTimestep);
		944	MoveAngular(pTimestep);
		945
		946	LimitRotation(pTimestep);
		947
		948	// remember the position so next step we can limit absolute movement effects
		949	m_lastPositionVector = VehiclePosition;
		950
		951	// If we forced the changing of some vehicle parameters, update the values and
		952	// for the physics engine to note the changes so an UpdateProperties event will happen.
		953	PushKnownChanged();
		954
		955	if (m_physicsScene.VehiclePhysicalLoggingEnabled)
		956	m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
		957
		958	VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
		959	ControllingPrim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
		960	}
		961
		962	// Called after the simulation step
		963	internal void PostStep(float pTimestep)
		964	{
		965	if (!IsActive) return;
		966
		967	if (m_physicsScene.VehiclePhysicalLoggingEnabled)
		968	m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
		969	}
		970
		971	// Apply the effect of the linear motor and other linear motions (like hover and float).
		972	private void MoveLinear(float pTimestep)
		973	{
		974	ComputeLinearVelocity(pTimestep);
		975
		976	ComputeLinearDeflection(pTimestep);
		977
		978	ComputeLinearTerrainHeightCorrection(pTimestep);
		979
		980	ComputeLinearHover(pTimestep);
		981
		982	ComputeLinearBlockingEndPoint(pTimestep);
		983
		984	ComputeLinearMotorUp(pTimestep);
		985
		986	ApplyGravity(pTimestep);
		987
		988	// If not changing some axis, reduce out velocity
		989	if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
		990	{
		991	Vector3 vel = VehicleVelocity;
		992	if ((m_flags & (VehicleFlag.NO_X)) != 0)
		993	{
		994	vel.X = 0;
		995	}
		996	if ((m_flags & (VehicleFlag.NO_Y)) != 0)
		997	{
		998	vel.Y = 0;
		999	}
		1000	if ((m_flags & (VehicleFlag.NO_Z)) != 0)
		1001	{
		1002	vel.Z = 0;
		1003	}
		1004	VehicleVelocity = vel;
		1005	}
		1006
		1007	// ==================================================================
		1008	// Clamp high or low velocities
		1009	float newVelocityLengthSq = VehicleVelocity.LengthSquared();
		1010	if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared)
		1011	{
		1012	Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG
		1013	VehicleVelocity /= VehicleVelocity.Length();
		1014	VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
		1015	VDetailLog("{0}, MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}",
		1016	ControllingPrim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity);
		1017	}
		1018	else if (newVelocityLengthSq < BSParam.VehicleMinLinearVelocitySquared)
		1019	{
		1020	Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG
		1021	VDetailLog("{0}, MoveLinear,clampMin,origVelW={1},lenSq={2}",
		1022	ControllingPrim.LocalID, origVelW, newVelocityLengthSq);
		1023	VehicleVelocity = Vector3.Zero;
		1024	}
		1025
		1026	VDetailLog("{0}, MoveLinear,done,isColl={1},newVel={2}", ControllingPrim.LocalID, ControllingPrim.HasSomeCollision, VehicleVelocity );
		1027
		1028	} // end MoveLinear()
		1029
		1030	public void ComputeLinearVelocity(float pTimestep)
		1031	{
		1032	// Step the motor from the current value. Get the correction needed this step.
		1033	Vector3 origVelW = VehicleVelocity; // DEBUG
		1034	Vector3 currentVelV = VehicleForwardVelocity;
		1035	Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV);
		1036
		1037	// Friction reduces vehicle motion based on absolute speed. Slow vehicle down by friction.
		1038	Vector3 frictionFactorV = ComputeFrictionFactor(m_linearFrictionTimescale, pTimestep);
		1039	linearMotorCorrectionV -= (currentVelV * frictionFactorV);
		1040
		1041	// Motor is vehicle coordinates. Rotate it to world coordinates
		1042	Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
		1043
		1044	// If we're a ground vehicle, don't add any upward Z movement
		1045	if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
		1046	{
		1047	if (linearMotorVelocityW.Z > 0f)
		1048	linearMotorVelocityW.Z = 0f;
		1049	}
		1050
		1051	// Add this correction to the velocity to make it faster/slower.
		1052	VehicleVelocity += linearMotorVelocityW;
		1053
		1054	VDetailLog("{0}, MoveLinear,velocity,origVelW={1},velV={2},tgt={3},correctV={4},correctW={5},newVelW={6},fricFact={7}",
		1055	ControllingPrim.LocalID, origVelW, currentVelV, m_linearMotor.TargetValue, linearMotorCorrectionV,
		1056	linearMotorVelocityW, VehicleVelocity, frictionFactorV);
		1057	}
		1058
		1059	//Given a Deflection Effiency and a Velocity, Returns a Velocity that is Partially Deflected onto the X Axis
		1060	//Clamped so that a DeflectionTimescale of less then 1 does not increase force over original velocity
		1061	private void ComputeLinearDeflection(float pTimestep)
		1062	{
		1063	Vector3 linearDeflectionV = Vector3.Zero;
		1064	Vector3 velocityV = VehicleForwardVelocity;
		1065
		1066	if (BSParam.VehicleEnableLinearDeflection)
		1067	{
		1068	// Velocity in Y and Z dimensions is movement to the side or turning.
		1069	// Compute deflection factor from the to the side and rotational velocity
		1070	linearDeflectionV.Y = SortedClampInRange(0, (velocityV.Y * m_linearDeflectionEfficiency) / m_linearDeflectionTimescale, velocityV.Y);
		1071	linearDeflectionV.Z = SortedClampInRange(0, (velocityV.Z * m_linearDeflectionEfficiency) / m_linearDeflectionTimescale, velocityV.Z);
		1072
		1073	// Velocity to the side and around is corrected and moved into the forward direction
		1074	linearDeflectionV.X += Math.Abs(linearDeflectionV.Y);
		1075	linearDeflectionV.X += Math.Abs(linearDeflectionV.Z);
		1076
		1077	// Scale the deflection to the fractional simulation time
		1078	linearDeflectionV *= pTimestep;
		1079
		1080	// Subtract the sideways and rotational velocity deflection factors while adding the correction forward
		1081	linearDeflectionV *= new Vector3(1, -1, -1);
		1082
		1083	// Correction is vehicle relative. Convert to world coordinates.
		1084	Vector3 linearDeflectionW = linearDeflectionV * VehicleOrientation;
		1085
		1086	// Optionally, if not colliding, don't effect world downward velocity. Let falling things fall.
		1087	if (BSParam.VehicleLinearDeflectionNotCollidingNoZ && !m_controllingPrim.HasSomeCollision)
		1088	{
		1089	linearDeflectionW.Z = 0f;
		1090	}
		1091
		1092	VehicleVelocity += linearDeflectionW;
		1093
		1094	VDetailLog("{0}, MoveLinear,LinearDeflection,linDefEff={1},linDefTS={2},linDeflectionV={3}",
		1095	ControllingPrim.LocalID, m_linearDeflectionEfficiency, m_linearDeflectionTimescale, linearDeflectionV);
		1096	}
		1097	}
		1098
		1099	public void ComputeLinearTerrainHeightCorrection(float pTimestep)
		1100	{
		1101	// If below the terrain, move us above the ground a little.
		1102	// TODO: Consider taking the rotated size of the object or possibly casting a ray.
		1103	if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
		1104	{
		1105	// Force position because applying force won't get the vehicle through the terrain
		1106	Vector3 newPosition = VehiclePosition;
		1107	newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
		1108	VehiclePosition = newPosition;
		1109	VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
		1110	ControllingPrim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
		1111	}
		1112	}
		1113
		1114	public void ComputeLinearHover(float pTimestep)
		1115	{
		1116	// m_VhoverEfficiency: 0=bouncy, 1=totally damped
		1117	// m_VhoverTimescale: time to achieve height
		1118	if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0 && (m_VhoverHeight > 0) && (m_VhoverTimescale < 300))
		1119	{
		1120	// We should hover, get the target height
		1121	if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
		1122	{
		1123	m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
		1124	}
		1125	if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
		1126	{
		1127	m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
		1128	}
		1129	if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
		1130	{
		1131	m_VhoverTargetHeight = m_VhoverHeight;
		1132	}
		1133	if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
		1134	{
		1135	// If body is already heigher, use its height as target height
		1136	if (VehiclePosition.Z > m_VhoverTargetHeight)
		1137	{
		1138	m_VhoverTargetHeight = VehiclePosition.Z;
		1139
		1140	// A 'misfeature' of this flag is that if the vehicle is above it's hover height,
		1141	// the vehicle's buoyancy goes away. This is an SL bug that got used by so many
		1142	// scripts that it could not be changed.
		1143	// So, if above the height, reapply gravity if buoyancy had it turned off.
		1144	if (m_VehicleBuoyancy != 0)
		1145	{
		1146	Vector3 appliedGravity = ControllingPrim.ComputeGravity(ControllingPrim.Buoyancy) * m_vehicleMass;
		1147	VehicleAddForce(appliedGravity);
		1148	}
		1149	}
		1150	}
		1151
		1152	if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
		1153	{
		1154	if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
		1155	{
		1156	Vector3 pos = VehiclePosition;
		1157	pos.Z = m_VhoverTargetHeight;
		1158	VehiclePosition = pos;
		1159
		1160	VDetailLog("{0}, MoveLinear,hover,pos={1},lockHoverHeight", ControllingPrim.LocalID, pos);
		1161	}
		1162	}
		1163	else
		1164	{
		1165	// Error is positive if below the target and negative if above.
		1166	Vector3 hpos = VehiclePosition;
		1167	float verticalError = m_VhoverTargetHeight - hpos.Z;
		1168	float verticalCorrection = verticalError / m_VhoverTimescale;
		1169	verticalCorrection *= m_VhoverEfficiency;
		1170
		1171	hpos.Z += verticalCorrection;
		1172	VehiclePosition = hpos;
		1173
		1174	// Since we are hovering, we need to do the opposite of falling -- get rid of world Z
		1175	Vector3 vel = VehicleVelocity;
		1176	vel.Z = 0f;
		1177	VehicleVelocity = vel;
		1178
		1179	/*
		1180	float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
		1181	Vector3 verticalCorrection = new Vector3(0f, 0f, verticalCorrectionVelocity);
		1182	verticalCorrection *= m_vehicleMass;
		1183
		1184	// TODO: implement m_VhoverEfficiency correctly
		1185	VehicleAddForceImpulse(verticalCorrection);
		1186	*/
		1187
		1188	VDetailLog("{0}, MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}",
		1189	ControllingPrim.LocalID, VehiclePosition, m_VhoverEfficiency,
		1190	m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight,
		1191	verticalError, verticalCorrection);
		1192	}
		1193	}
		1194	}
		1195
		1196	public bool ComputeLinearBlockingEndPoint(float pTimestep)
		1197	{
		1198	bool changed = false;
		1199
		1200	Vector3 pos = VehiclePosition;
		1201	Vector3 posChange = pos - m_lastPositionVector;
		1202	if (m_BlockingEndPoint != Vector3.Zero)
		1203	{
		1204	if (pos.X >= (m_BlockingEndPoint.X - (float)1))
		1205	{
		1206	pos.X -= posChange.X + 1;
		1207	changed = true;
		1208	}
		1209	if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
		1210	{
		1211	pos.Y -= posChange.Y + 1;
		1212	changed = true;
		1213	}
		1214	if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
		1215	{
		1216	pos.Z -= posChange.Z + 1;
		1217	changed = true;
		1218	}
		1219	if (pos.X <= 0)
		1220	{
		1221	pos.X += posChange.X + 1;
		1222	changed = true;
		1223	}
		1224	if (pos.Y <= 0)
		1225	{
		1226	pos.Y += posChange.Y + 1;
		1227	changed = true;
		1228	}
		1229	if (changed)
		1230	{
		1231	VehiclePosition = pos;
		1232	VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
		1233	ControllingPrim.LocalID, m_BlockingEndPoint, posChange, pos);
		1234	}
		1235	}
		1236	return changed;
		1237	}
		1238
		1239	// From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
		1240	// Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
		1241	// used with conjunction with banking: the strength of the banking will decay when the
		1242	// vehicle no longer experiences collisions. The decay timescale is the same as
		1243	// VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
		1244	// when they are in mid jump.
		1245	// TODO: this code is wrong. Also, what should it do for boats (height from water)?
		1246	// This is just using the ground and a general collision check. Should really be using
		1247	// a downward raycast to find what is below.
		1248	public void ComputeLinearMotorUp(float pTimestep)
		1249	{
		1250	if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
		1251	{
		1252	// This code tries to decide if the object is not on the ground and then pushing down
		1253	/*
		1254	float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
		1255	distanceAboveGround = VehiclePosition.Z - targetHeight;
		1256	// Not colliding if the vehicle is off the ground
		1257	if (!Prim.HasSomeCollision)
		1258	{
		1259	// downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
		1260	VehicleVelocity += new Vector3(0, 0, -distanceAboveGround);
		1261	}
		1262	// TODO: this calculation is wrong. From the description at
		1263	// (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
		1264	// has a decay factor. This says this force should
		1265	// be computed with a motor.
		1266	// TODO: add interaction with banking.
		1267	VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
		1268	Prim.LocalID, distanceAboveGround, Prim.HasSomeCollision, ret);
		1269	*/
		1270
		1271	// Another approach is to measure if we're going up. If going up and not colliding,
		1272	// the vehicle is in the air. Fix that by pushing down.
		1273	if (!ControllingPrim.HasSomeCollision && VehicleVelocity.Z > 0.1)
		1274	{
		1275	// Get rid of any of the velocity vector that is pushing us up.
		1276	float upVelocity = VehicleVelocity.Z;
		1277	VehicleVelocity += new Vector3(0, 0, -upVelocity);
		1278
		1279	/*
		1280	// If we're pointed up into the air, we should nose down
		1281	Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
		1282	// The rotation around the Y axis is pitch up or down
		1283	if (pointingDirection.Y > 0.01f)
		1284	{
		1285	float angularCorrectionForce = -(float)Math.Asin(pointingDirection.Y);
		1286	Vector3 angularCorrectionVector = new Vector3(0f, angularCorrectionForce, 0f);
		1287	// Rotate into world coordinates and apply to vehicle
		1288	angularCorrectionVector *= VehicleOrientation;
		1289	VehicleAddAngularForce(angularCorrectionVector);
		1290	VDetailLog("{0}, MoveLinear,limitMotorUp,newVel={1},pntDir={2},corrFrc={3},aCorr={4}",
		1291	Prim.LocalID, VehicleVelocity, pointingDirection, angularCorrectionForce, angularCorrectionVector);
		1292	}
		1293	*/
		1294	VDetailLog("{0}, MoveLinear,limitMotorUp,collide={1},upVel={2},newVel={3}",
		1295	ControllingPrim.LocalID, ControllingPrim.HasSomeCollision, upVelocity, VehicleVelocity);
		1296	}
		1297	}
		1298	}
		1299
		1300	private void ApplyGravity(float pTimeStep)
		1301	{
		1302	Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
		1303
		1304	// Hack to reduce downward force if the vehicle is probably sitting on the ground
		1305	if (ControllingPrim.HasSomeCollision && IsGroundVehicle)
		1306	appliedGravity *= BSParam.VehicleGroundGravityFudge;
		1307
		1308	VehicleAddForce(appliedGravity);
		1309
		1310	VDetailLog("{0}, MoveLinear,applyGravity,vehGrav={1},collid={2},fudge={3},mass={4},appliedForce={5}",
		1311	ControllingPrim.LocalID, m_VehicleGravity,
		1312	ControllingPrim.HasSomeCollision, BSParam.VehicleGroundGravityFudge, m_vehicleMass, appliedGravity);
		1313	}
		1314
		1315	// =======================================================================
		1316	// =======================================================================
		1317	// Apply the effect of the angular motor.
		1318	// The 'contribution' is how much angular correction velocity each function wants.
		1319	// All the contributions are added together and the resulting velocity is
		1320	// set directly on the vehicle.
		1321	private void MoveAngular(float pTimestep)
		1322	{
		1323	ComputeAngularTurning(pTimestep);
		1324
		1325	ComputeAngularVerticalAttraction();
		1326
		1327	ComputeAngularDeflection();
		1328
		1329	ComputeAngularBanking();
		1330
		1331	// ==================================================================
		1332	if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
		1333	{
		1334	// The vehicle is not adding anything angular wise.
		1335	VehicleRotationalVelocity = Vector3.Zero;
		1336	VDetailLog("{0}, MoveAngular,done,zero", ControllingPrim.LocalID);
		1337	}
		1338	else
		1339	{
		1340	VDetailLog("{0}, MoveAngular,done,nonZero,angVel={1}", ControllingPrim.LocalID, VehicleRotationalVelocity);
		1341	}
		1342
		1343	// ==================================================================
		1344	//Offset section
		1345	if (m_linearMotorOffset != Vector3.Zero)
		1346	{
		1347	//Offset of linear velocity doesn't change the linear velocity,
		1348	// but causes a torque to be applied, for example...
		1349	//
		1350	// IIIII >>> IIIII
		1351	// IIIII >>> IIIII
		1352	// IIIII >>> IIIII
		1353	// ^
		1354	// | Applying a force at the arrow will cause the object to move forward, but also rotate
		1355	//
		1356	//
		1357	// The torque created is the linear velocity crossed with the offset
		1358
		1359	// TODO: this computation should be in the linear section
		1360	// because that is where we know the impulse being applied.
		1361	Vector3 torqueFromOffset = Vector3.Zero;
		1362	// torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
		1363	if (float.IsNaN(torqueFromOffset.X))
		1364	torqueFromOffset.X = 0;
		1365	if (float.IsNaN(torqueFromOffset.Y))
		1366	torqueFromOffset.Y = 0;
		1367	if (float.IsNaN(torqueFromOffset.Z))
		1368	torqueFromOffset.Z = 0;
		1369
		1370	VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
		1371	VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", ControllingPrim.LocalID, torqueFromOffset);
		1372	}
		1373
		1374	}
		1375
		1376	private void ComputeAngularTurning(float pTimestep)
		1377	{
		1378	// The user wants this many radians per second angular change?
		1379	Vector3 origVehicleRotationalVelocity = VehicleRotationalVelocity; // DEBUG DEBUG
		1380	Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
		1381	Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
		1382
		1383	// ==================================================================
		1384	// From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
		1385	// This flag prevents linear deflection parallel to world z-axis. This is useful
		1386	// for preventing ground vehicles with large linear deflection, like bumper cars,
		1387	// from climbing their linear deflection into the sky.
		1388	// That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
		1389	// TODO: This is here because this is where ODE put it but documentation says it
		1390	// is a linear effect. Where should this check go?
		1391	//if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
		1392	// {
		1393	// angularMotorContributionV.X = 0f;
		1394	// angularMotorContributionV.Y = 0f;
		1395	// }
		1396
		1397	// Reduce any velocity by friction.
		1398	Vector3 frictionFactorW = ComputeFrictionFactor(m_angularFrictionTimescale, pTimestep);
		1399	angularMotorContributionV -= (currentAngularV * frictionFactorW);
		1400
		1401	Vector3 angularMotorContributionW = angularMotorContributionV * VehicleOrientation;
		1402	VehicleRotationalVelocity += angularMotorContributionW;
		1403
		1404	VDetailLog("{0}, MoveAngular,angularTurning,curAngVelV={1},origVehRotVel={2},vehRotVel={3},frictFact={4}, angContribV={5},angContribW={6}",
		1405	ControllingPrim.LocalID, currentAngularV, origVehicleRotationalVelocity, VehicleRotationalVelocity, frictionFactorW, angularMotorContributionV, angularMotorContributionW);
		1406	}
		1407
		1408	// From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
		1409	// Some vehicles, like boats, should always keep their up-side up. This can be done by
		1410	// enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
		1411	// the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
		1412	// VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
		1413	// and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
		1414	// efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
		1415	// efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
		1416	public void ComputeAngularVerticalAttraction()
		1417	{
		1418
		1419	// If vertical attaction timescale is reasonable
		1420	if (BSParam.VehicleEnableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
		1421	{
		1422	Vector3 vehicleUpAxis = Vector3.UnitZ * VehicleOrientation;
		1423	switch (BSParam.VehicleAngularVerticalAttractionAlgorithm)
		1424	{
		1425	case 0:
		1426	{
		1427	//Another formula to try got from :
		1428	//http://answers.unity3d.com/questions/10425/how-to-stabilize-angular-motion-alignment-of-hover.html
		1429
		1430	// Flipping what was originally a timescale into a speed variable and then multiplying it by 2
		1431	// since only computing half the distance between the angles.
		1432	float verticalAttractionSpeed = (1 / m_verticalAttractionTimescale) * 2.0f;
		1433
		1434	// Make a prediction of where the up axis will be when this is applied rather then where it is now as
		1435	// this makes for a smoother adjustment and less fighting between the various forces.
		1436	Vector3 predictedUp = vehicleUpAxis * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
		1437
		1438	// This is only half the distance to the target so it will take 2 seconds to complete the turn.
		1439	Vector3 torqueVector = Vector3.Cross(predictedUp, Vector3.UnitZ);
		1440
		1441	if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != 0)
		1442	{
		1443	Vector3 vehicleForwardAxis = Vector3.UnitX * VehicleOrientation;
		1444	torqueVector = ProjectVector(torqueVector, vehicleForwardAxis);
		1445	}
		1446
		1447	// Scale vector by our timescale since it is an acceleration it is r/s^2 or radians a timescale squared
		1448	Vector3 vertContributionV = torqueVector * verticalAttractionSpeed * verticalAttractionSpeed;
		1449
		1450	VehicleRotationalVelocity += vertContributionV;
		1451
		1452	VDetailLog("{0}, MoveAngular,verticalAttraction,vertAttrSpeed={1},upAxis={2},PredictedUp={3},torqueVector={4},contrib={5}",
		1453	ControllingPrim.LocalID,
		1454	verticalAttractionSpeed,
		1455	vehicleUpAxis,
		1456	predictedUp,
		1457	torqueVector,
		1458	vertContributionV);
		1459	break;
		1460	}
		1461	case 1:
		1462	{
		1463	// Possible solution derived from a discussion at:
		1464	// http://stackoverflow.com/questions/14939657/computing-vector-from-quaternion-works-computing-quaternion-from-vector-does-no
		1465
		1466	// Create a rotation that is only the vehicle's rotation around Z
		1467	Vector3 currentEulerW = Vector3.Zero;
		1468	VehicleOrientation.GetEulerAngles(out currentEulerW.X, out currentEulerW.Y, out currentEulerW.Z);
		1469	Quaternion justZOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, currentEulerW.Z);
		1470
		1471	// Create the axis that is perpendicular to the up vector and the rotated up vector.
		1472	Vector3 differenceAxisW = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleOrientation);
		1473	// Compute the angle between those to vectors.
		1474	double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleOrientation)));
		1475	// 'differenceAngle' is the angle to rotate and 'differenceAxis' is the plane to rotate in to get the vehicle vertical
		1476
		1477	// Reduce the change by the time period it is to change in. Timestep is handled when velocity is applied.
		1478	// TODO: add 'efficiency'.
		1479	// differenceAngle /= m_verticalAttractionTimescale;
		1480
		1481	// Create the quaterian representing the correction angle
		1482	Quaternion correctionRotationW = Quaternion.CreateFromAxisAngle(differenceAxisW, (float)differenceAngle);
		1483
		1484	// Turn that quaternion into Euler values to make it into velocities to apply.
		1485	Vector3 vertContributionW = Vector3.Zero;
		1486	correctionRotationW.GetEulerAngles(out vertContributionW.X, out vertContributionW.Y, out vertContributionW.Z);
		1487	vertContributionW *= -1f;
		1488	vertContributionW /= m_verticalAttractionTimescale;
		1489
		1490	VehicleRotationalVelocity += vertContributionW;
		1491
		1492	VDetailLog("{0}, MoveAngular,verticalAttraction,upAxis={1},diffAxis={2},diffAng={3},corrRot={4},contrib={5}",
		1493	ControllingPrim.LocalID,
		1494	vehicleUpAxis,
		1495	differenceAxisW,
		1496	differenceAngle,
		1497	correctionRotationW,
		1498	vertContributionW);
		1499	break;
		1500	}
		1501	case 2:
		1502	{
		1503	Vector3 vertContributionV = Vector3.Zero;
		1504	Vector3 origRotVelW = VehicleRotationalVelocity; // DEBUG DEBUG
		1505
		1506	// Take a vector pointing up and convert it from world to vehicle relative coords.
		1507	Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleOrientation);
		1508
		1509	// If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
		1510	// is now:
		1511	// leaning to one side: rotated around the X axis with the Y value going
		1512	// from zero (nearly straight up) to one (completely to the side)) or
		1513	// leaning front-to-back: rotated around the Y axis with the value of X being between
		1514	// zero and one.
		1515	// The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
		1516
		1517	// Y error means needed rotation around X axis and visa versa.
		1518	// Since the error goes from zero to one, the asin is the corresponding angle.
		1519	vertContributionV.X = (float)Math.Asin(verticalError.Y);
		1520	// (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
		1521	vertContributionV.Y = -(float)Math.Asin(verticalError.X);
		1522
		1523	// If verticalError.Z is negative, the vehicle is upside down. Add additional push.
		1524	if (verticalError.Z < 0f)
		1525	{
		1526	vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour;
		1527	// vertContribution.Y -= PIOverFour;
		1528	}
		1529
		1530	// 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
		1531	// Correction happens over a number of seconds.
		1532	Vector3 unscaledContribVerticalErrorV = vertContributionV; // DEBUG DEBUG
		1533
		1534	// The correction happens over the user's time period
		1535	vertContributionV /= m_verticalAttractionTimescale;
		1536
		1537	// Rotate the vehicle rotation to the world coordinates.
		1538	VehicleRotationalVelocity += (vertContributionV * VehicleOrientation);
		1539
		1540	VDetailLog("{0}, MoveAngular,verticalAttraction,,upAxis={1},origRotVW={2},vertError={3},unscaledV={4},eff={5},ts={6},vertContribV={7}",
		1541	ControllingPrim.LocalID,
		1542	vehicleUpAxis,
		1543	origRotVelW,
		1544	verticalError,
		1545	unscaledContribVerticalErrorV,
		1546	m_verticalAttractionEfficiency,
		1547	m_verticalAttractionTimescale,
		1548	vertContributionV);
		1549	break;
		1550	}
		1551	default:
		1552	{
		1553	break;
		1554	}
		1555	}
		1556	}
		1557	}
		1558
		1559	// Angular correction to correct the direction the vehicle is pointing to be
		1560	// the direction is should want to be pointing.
		1561	// The vehicle is moving in some direction and correct its orientation to it is pointing
		1562	// in that direction.
		1563	// TODO: implement reference frame.
		1564	public void ComputeAngularDeflection()
		1565	{
		1566
		1567	if (BSParam.VehicleEnableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
		1568	{
		1569	Vector3 deflectContributionV = Vector3.Zero;
		1570
		1571	// The direction the vehicle is moving
		1572	Vector3 movingDirection = VehicleVelocity;
		1573	movingDirection.Normalize();
		1574
		1575	// If the vehicle is going backward, it is still pointing forward
		1576	movingDirection *= Math.Sign(VehicleForwardSpeed);
		1577
		1578	// The direction the vehicle is pointing
		1579	Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
		1580	//Predict where the Vehicle will be pointing after AngularVelocity change is applied. This will keep
		1581	// from overshooting and allow this correction to merge with the Vertical Attraction peacefully.
		1582	Vector3 predictedPointingDirection = pointingDirection * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
		1583	predictedPointingDirection.Normalize();
		1584
		1585	// The difference between what is and what should be.
		1586	// Vector3 deflectionError = movingDirection - predictedPointingDirection;
		1587	Vector3 deflectionError = Vector3.Cross(movingDirection, predictedPointingDirection);
		1588
		1589	// Don't try to correct very large errors (not our job)
		1590	// if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
		1591	// if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y);
		1592	// if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z);
		1593	if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
		1594	if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
		1595	if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
		1596
		1597	// ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
		1598
		1599	// Scale the correction by recovery timescale and efficiency
		1600	// Not modeling a spring so clamp the scale to no more then the arc
		1601	deflectContributionV = (-deflectionError) * ClampInRange(0, m_angularDeflectionEfficiency/m_angularDeflectionTimescale,1f);
		1602	//deflectContributionV /= m_angularDeflectionTimescale;
		1603
		1604	// VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
		1605	VehicleRotationalVelocity += deflectContributionV;
		1606	VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
		1607	ControllingPrim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
		1608	VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3},PredictedPointingDir={4}",
		1609	ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale, predictedPointingDirection);
		1610	}
		1611	}
		1612
		1613	// Angular change to rotate the vehicle around the Z axis when the vehicle
		1614	// is tipped around the X axis.
		1615	// From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
		1616	// The vertical attractor feature must be enabled in order for the banking behavior to
		1617	// function. The way banking works is this: a rotation around the vehicle's roll-axis will
		1618	// produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
		1619	// of the yaw effect will be proportional to the
		1620	// VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
		1621	// velocity along its preferred axis of motion.
		1622	// The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
		1623	// positive rotation (by the right-hand rule) about the roll-axis will effect a
		1624	// (negative) torque around the yaw-axis, making it turn to the right--that is the
		1625	// vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
		1626	// Negating the banking coefficient will make it so that the vehicle leans to the
		1627	// outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
		1628	// The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
		1629	// banking vehicles do what you want rather than what the laws of physics allow.
		1630	// For example, consider a real motorcycle...it must be moving forward in order for
		1631	// it to turn while banking, however video-game motorcycles are often configured
		1632	// to turn in place when at a dead stop--because they are often easier to control
		1633	// that way using the limited interface of the keyboard or game controller. The
		1634	// VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
		1635	// banking by functioning as a slider between a banking that is correspondingly
		1636	// totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
		1637	// banking effect depends only on the vehicle's rotation about its roll-axis compared
		1638	// to "dynamic" where the banking is also proportional to its velocity along its
		1639	// roll-axis. Finding the best value of the "mixture" will probably require trial and error.
		1640	// The time it takes for the banking behavior to defeat a preexisting angular velocity about the
		1641	// world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
		1642	// bank quickly then give it a banking timescale of about a second or less, otherwise you can
		1643	// make a sluggish vehicle by giving it a timescale of several seconds.
		1644	public void ComputeAngularBanking()
		1645	{
		1646	if (BSParam.VehicleEnableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
		1647	{
		1648	Vector3 bankingContributionV = Vector3.Zero;
		1649
		1650	// Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
		1651	// As the vehicle rolls to the right or left, the Y value will increase from
		1652	// zero (straight up) to 1 or -1 (full tilt right or left)
		1653	Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
		1654
		1655	// Figure out the yaw value for this much roll.
		1656	float yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;
		1657	// actual error = static turn error + dynamic turn error
		1658	float mixedYawAngle =(yawAngle * (1f - m_bankingMix)) + ((yawAngle * m_bankingMix) * VehicleForwardSpeed);
		1659
		1660	// TODO: the banking effect should not go to infinity but what to limit it to?
		1661	// And what should happen when this is being added to a user defined yaw that is already PI*4?
		1662	mixedYawAngle = ClampInRange(-12, mixedYawAngle, 12);
		1663
		1664	// Build the force vector to change rotation from what it is to what it should be
		1665	bankingContributionV.Z = -mixedYawAngle;
		1666
		1667	// Don't do it all at once. Fudge because 1 second is too fast with most user defined roll as PI*4.
		1668	bankingContributionV /= m_bankingTimescale * BSParam.VehicleAngularBankingTimescaleFudge;
		1669
		1670	//VehicleRotationalVelocity += bankingContributionV * VehicleOrientation;
		1671	VehicleRotationalVelocity += bankingContributionV;
		1672
		1673
		1674	VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
		1675	ControllingPrim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
		1676	}
		1677	}
		1678
		1679	// This is from previous instantiations of XXXDynamics.cs.
		1680	// Applies roll reference frame.
		1681	// TODO: is this the right way to separate the code to do this operation?
		1682	// Should this be in MoveAngular()?
		1683	internal void LimitRotation(float timestep)
		1684	{
		1685	Quaternion rotq = VehicleOrientation;
		1686	Quaternion m_rot = rotq;
		1687	if (m_RollreferenceFrame != Quaternion.Identity)
		1688	{
		1689	if (rotq.X >= m_RollreferenceFrame.X)
		1690	{
		1691	m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
		1692	}
		1693	if (rotq.Y >= m_RollreferenceFrame.Y)
		1694	{
		1695	m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
		1696	}
		1697	if (rotq.X <= -m_RollreferenceFrame.X)
		1698	{
		1699	m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
		1700	}
		1701	if (rotq.Y <= -m_RollreferenceFrame.Y)
		1702	{
		1703	m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
		1704	}
		1705	}
		1706	if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
		1707	{
		1708	m_rot.X = 0;
		1709	m_rot.Y = 0;
		1710	}
		1711	if (rotq != m_rot)
		1712	{
		1713	VehicleOrientation = m_rot;
		1714	VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", ControllingPrim.LocalID, rotq, m_rot);
		1715	}
		1716
		1717	}
		1718
		1719	// Given a friction vector (reduction in seconds) and a timestep, return the factor to reduce
		1720	// some value by to apply this friction.
		1721	private Vector3 ComputeFrictionFactor(Vector3 friction, float pTimestep)
		1722	{
		1723	Vector3 frictionFactor = Vector3.Zero;
		1724	if (friction != BSMotor.InfiniteVector)
		1725	{
		1726	// frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
		1727	// Individual friction components can be 'infinite' so compute each separately.
		1728	frictionFactor.X = (friction.X == BSMotor.Infinite) ? 0f : (1f / friction.X);
		1729	frictionFactor.Y = (friction.Y == BSMotor.Infinite) ? 0f : (1f / friction.Y);
		1730	frictionFactor.Z = (friction.Z == BSMotor.Infinite) ? 0f : (1f / friction.Z);
		1731	frictionFactor *= pTimestep;
		1732	}
		1733	return frictionFactor;
		1734	}
		1735
		1736	private float SortedClampInRange(float clampa, float val, float clampb)
		1737	{
		1738	if (clampa > clampb)
		1739	{
		1740	float temp = clampa;
		1741	clampa = clampb;
		1742	clampb = temp;
		1743	}
		1744	return ClampInRange(clampa, val, clampb);
		1745
		1746	}
		1747
		1748	//Given a Vector and a unit vector will return the amount of the vector is on the same axis as the unit.
		1749	private Vector3 ProjectVector(Vector3 vector, Vector3 onNormal)
		1750	{
		1751	float vectorDot = Vector3.Dot(vector, onNormal);
		1752	return onNormal * vectorDot;
		1753
		1754	}
		1755
		1756	private float ClampInRange(float low, float val, float high)
		1757	{
		1758	return Math.Max(low, Math.Min(val, high));
		1759	// return Utils.Clamp(val, low, high);
		1760	}
		1761
		1762	// Invoke the detailed logger and output something if it's enabled.
		1763	private void VDetailLog(string msg, params Object[] args)
		1764	{
		1765	if (ControllingPrim.PhysScene.VehicleLoggingEnabled)
		1766	ControllingPrim.PhysScene.DetailLog(msg, args);
		1767	}
		1768	}
		1769	}