diff options
author | onefang | 2019-05-19 21:24:15 +1000 |
---|---|---|
committer | onefang | 2019-05-19 21:24:15 +1000 |
commit | 5e4d6cab00cb29cd088ab7b62ab13aff103b64cb (patch) | |
tree | a9fbc62df9eb2d1d9ba2698d8552eae71eca20d8 /OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | |
parent | Add a build script. (diff) | |
download | opensim-SC_OLD-5e4d6cab00cb29cd088ab7b62ab13aff103b64cb.zip opensim-SC_OLD-5e4d6cab00cb29cd088ab7b62ab13aff103b64cb.tar.gz opensim-SC_OLD-5e4d6cab00cb29cd088ab7b62ab13aff103b64cb.tar.bz2 opensim-SC_OLD-5e4d6cab00cb29cd088ab7b62ab13aff103b64cb.tar.xz |
Dump OpenSim 0.9.0.1 into it's own branch.
Diffstat (limited to 'OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | 720 |
1 files changed, 272 insertions, 448 deletions
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs index 0b9c45f..8934330 100644 --- a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs +++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | |||
@@ -33,7 +33,7 @@ | |||
33 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | 33 | * ODEDynamics.cs contains methods dealing with Prim Physical motion |
34 | * (dynamics) and the associated settings. Old Linear and angular | 34 | * (dynamics) and the associated settings. Old Linear and angular |
35 | * motors for dynamic motion have been replace with MoveLinear() | 35 | * motors for dynamic motion have been replace with MoveLinear() |
36 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | 36 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic |
37 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | 37 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to |
38 | * switch between 'VEHICLE' parameter use and general dynamics | 38 | * switch between 'VEHICLE' parameter use and general dynamics |
39 | * settings use. | 39 | * settings use. |
@@ -48,7 +48,6 @@ using System.Runtime.InteropServices; | |||
48 | using System.Threading; | 48 | using System.Threading; |
49 | using log4net; | 49 | using log4net; |
50 | using OpenMetaverse; | 50 | using OpenMetaverse; |
51 | using Ode.NET; | ||
52 | using OpenSim.Framework; | 51 | using OpenSim.Framework; |
53 | using OpenSim.Region.PhysicsModules.SharedBase; | 52 | using OpenSim.Region.PhysicsModules.SharedBase; |
54 | 53 | ||
@@ -83,8 +82,15 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
83 | set | 82 | set |
84 | { | 83 | { |
85 | m_isphysical = value; | 84 | m_isphysical = value; |
86 | if (!m_isphysical) // Zero the remembered last velocity | 85 | if (!m_isphysical) |
86 | { | ||
87 | _zeroFlag = true; // Zero the remembered last velocity | ||
87 | m_lastVelocity = Vector3.Zero; | 88 | m_lastVelocity = Vector3.Zero; |
89 | _acceleration = Vector3.Zero; | ||
90 | _velocity = Vector3.Zero; | ||
91 | m_taintVelocity = Vector3.Zero; | ||
92 | m_rotationalVelocity = Vector3.Zero; | ||
93 | } | ||
88 | } | 94 | } |
89 | } | 95 | } |
90 | 96 | ||
@@ -104,10 +110,12 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
104 | private Vector3 m_taintVelocity; | 110 | private Vector3 m_taintVelocity; |
105 | private Vector3 m_taintTorque; | 111 | private Vector3 m_taintTorque; |
106 | private Quaternion m_taintrot; | 112 | private Quaternion m_taintrot; |
107 | private Vector3 m_angularlock = Vector3.One; | 113 | |
108 | private Vector3 m_taintAngularLock = Vector3.One; | ||
109 | private IntPtr Amotor = IntPtr.Zero; | 114 | private IntPtr Amotor = IntPtr.Zero; |
110 | 115 | ||
116 | private byte m_taintAngularLock = 0; | ||
117 | private byte m_angularlock = 0; | ||
118 | |||
111 | private bool m_assetFailed = false; | 119 | private bool m_assetFailed = false; |
112 | 120 | ||
113 | private Vector3 m_PIDTarget; | 121 | private Vector3 m_PIDTarget; |
@@ -125,7 +133,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
125 | private float m_targetHoverHeight; | 133 | private float m_targetHoverHeight; |
126 | private float m_groundHeight; | 134 | private float m_groundHeight; |
127 | private float m_waterHeight; | 135 | private float m_waterHeight; |
128 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 136 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. |
129 | 137 | ||
130 | // private float m_tensor = 5f; | 138 | // private float m_tensor = 5f; |
131 | private int body_autodisable_frames = 20; | 139 | private int body_autodisable_frames = 20; |
@@ -162,7 +170,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
162 | 170 | ||
163 | private PrimitiveBaseShape _pbs; | 171 | private PrimitiveBaseShape _pbs; |
164 | private OdeScene _parent_scene; | 172 | private OdeScene _parent_scene; |
165 | 173 | ||
166 | /// <summary> | 174 | /// <summary> |
167 | /// The physics space which contains prim geometries | 175 | /// The physics space which contains prim geometries |
168 | /// </summary> | 176 | /// </summary> |
@@ -195,7 +203,6 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
195 | public int m_interpenetrationcount { get; private set; } | 203 | public int m_interpenetrationcount { get; private set; } |
196 | internal float m_collisionscore; | 204 | internal float m_collisionscore; |
197 | public int m_roundsUnderMotionThreshold { get; private set; } | 205 | public int m_roundsUnderMotionThreshold { get; private set; } |
198 | private int m_crossingfailures; | ||
199 | 206 | ||
200 | public bool outofBounds { get; private set; } | 207 | public bool outofBounds { get; private set; } |
201 | private float m_density = 10.000006836f; // Aluminum g/cm3; | 208 | private float m_density = 10.000006836f; // Aluminum g/cm3; |
@@ -350,12 +357,12 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
350 | if (m_assetFailed) | 357 | if (m_assetFailed) |
351 | { | 358 | { |
352 | d.GeomSetCategoryBits(prim_geom, 0); | 359 | d.GeomSetCategoryBits(prim_geom, 0); |
353 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | 360 | d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); |
354 | } | 361 | } |
355 | else | 362 | else |
356 | { | 363 | { |
357 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 364 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
358 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 365 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
359 | } | 366 | } |
360 | 367 | ||
361 | _parent_scene.geom_name_map[prim_geom] = Name; | 368 | _parent_scene.geom_name_map[prim_geom] = Name; |
@@ -423,7 +430,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
423 | if (m_assetFailed) | 430 | if (m_assetFailed) |
424 | { | 431 | { |
425 | d.GeomSetCategoryBits(prim_geom, 0); | 432 | d.GeomSetCategoryBits(prim_geom, 0); |
426 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | 433 | d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); |
427 | } | 434 | } |
428 | else | 435 | else |
429 | { | 436 | { |
@@ -431,12 +438,12 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
431 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | 438 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); |
432 | } | 439 | } |
433 | 440 | ||
434 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 441 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
435 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 442 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
436 | 443 | ||
437 | d.BodySetAutoDisableFlag(Body, true); | 444 | d.BodySetAutoDisableFlag(Body, true); |
438 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | 445 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); |
439 | 446 | ||
440 | // disconnect from world gravity so we can apply buoyancy | 447 | // disconnect from world gravity so we can apply buoyancy |
441 | d.BodySetGravityMode (Body, false); | 448 | d.BodySetGravityMode (Body, false); |
442 | 449 | ||
@@ -445,7 +452,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
445 | m_disabled = false; | 452 | m_disabled = false; |
446 | 453 | ||
447 | // The body doesn't already have a finite rotation mode set here | 454 | // The body doesn't already have a finite rotation mode set here |
448 | if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) | 455 | if (m_angularlock != 0 && _parent == null) |
449 | { | 456 | { |
450 | createAMotor(m_angularlock); | 457 | createAMotor(m_angularlock); |
451 | } | 458 | } |
@@ -467,8 +474,8 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
467 | 474 | ||
468 | float returnMass = 0; | 475 | float returnMass = 0; |
469 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | 476 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; |
470 | float hollowVolume = hollowAmount * hollowAmount; | 477 | float hollowVolume = hollowAmount * hollowAmount; |
471 | 478 | ||
472 | switch (_pbs.ProfileShape) | 479 | switch (_pbs.ProfileShape) |
473 | { | 480 | { |
474 | case ProfileShape.Square: | 481 | case ProfileShape.Square: |
@@ -504,16 +511,16 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
504 | 511 | ||
505 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | 512 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) |
506 | { | 513 | { |
507 | //a tube | 514 | //a tube |
508 | 515 | ||
509 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | 516 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); |
510 | tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); | 517 | tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); |
511 | volume -= volume*tmp*tmp; | 518 | volume -= volume*tmp*tmp; |
512 | 519 | ||
513 | if (hollowAmount > 0.0) | 520 | if (hollowAmount > 0.0) |
514 | { | 521 | { |
515 | hollowVolume *= hollowAmount; | 522 | hollowVolume *= hollowAmount; |
516 | 523 | ||
517 | switch (_pbs.HollowShape) | 524 | switch (_pbs.HollowShape) |
518 | { | 525 | { |
519 | case HollowShape.Square: | 526 | case HollowShape.Square: |
@@ -572,7 +579,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
572 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | 579 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); |
573 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | 580 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); |
574 | volume *= (1.0f - tmp * tmp); | 581 | volume *= (1.0f - tmp * tmp); |
575 | 582 | ||
576 | if (hollowAmount > 0.0) | 583 | if (hollowAmount > 0.0) |
577 | { | 584 | { |
578 | 585 | ||
@@ -816,8 +823,8 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
816 | } | 823 | } |
817 | else | 824 | else |
818 | { | 825 | { |
819 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 826 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
820 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 827 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
821 | } | 828 | } |
822 | 829 | ||
823 | d.BodyDestroy(Body); | 830 | d.BodyDestroy(Body); |
@@ -838,7 +845,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
838 | else | 845 | else |
839 | { | 846 | { |
840 | _parent_scene.DeactivatePrim(this); | 847 | _parent_scene.DeactivatePrim(this); |
841 | 848 | ||
842 | m_collisionCategories &= ~CollisionCategories.Body; | 849 | m_collisionCategories &= ~CollisionCategories.Body; |
843 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | 850 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); |
844 | 851 | ||
@@ -850,8 +857,8 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
850 | else | 857 | else |
851 | { | 858 | { |
852 | 859 | ||
853 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 860 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
854 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 861 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
855 | } | 862 | } |
856 | 863 | ||
857 | Body = IntPtr.Zero; | 864 | Body = IntPtr.Zero; |
@@ -909,7 +916,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
909 | else | 916 | else |
910 | { | 917 | { |
911 | _triMeshData = d.GeomTriMeshDataCreate(); | 918 | _triMeshData = d.GeomTriMeshDataCreate(); |
912 | 919 | ||
913 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | 920 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); |
914 | d.GeomTriMeshDataPreprocess(_triMeshData); | 921 | d.GeomTriMeshDataPreprocess(_triMeshData); |
915 | m_MeshToTriMeshMap[mesh] = _triMeshData; | 922 | m_MeshToTriMeshMap[mesh] = _triMeshData; |
@@ -919,7 +926,7 @@ namespace OpenSim.Region.PhysicsModule.ODE | |||
919 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | 926 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); |
920 | try | 927 | try |
921 | { | 928 | { |
922 | SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); | 929 | SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); |
923 | } | 930 | } |
924 | catch (AccessViolationException) | 931 | catch (AccessViolationException) |
925 | { | 932 | { |
@@ -967,7 +974,7 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
967 | rotate(); | 974 | rotate(); |
968 | } | 975 | } |
969 | } | 976 | } |
970 | 977 | ||
971 | if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) | 978 | if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) |
972 | changePhysicsStatus(); | 979 | changePhysicsStatus(); |
973 | 980 | ||
@@ -1001,7 +1008,7 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1001 | if (m_taintCollidesWater != m_collidesWater) | 1008 | if (m_taintCollidesWater != m_collidesWater) |
1002 | changefloatonwater(); | 1009 | changefloatonwater(); |
1003 | 1010 | ||
1004 | if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) | 1011 | if (m_taintAngularLock != m_angularlock) |
1005 | changeAngularLock(); | 1012 | changeAngularLock(); |
1006 | } | 1013 | } |
1007 | 1014 | ||
@@ -1017,10 +1024,8 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1017 | //If we have a parent then we're not authorative here | 1024 | //If we have a parent then we're not authorative here |
1018 | if (_parent == null) | 1025 | if (_parent == null) |
1019 | { | 1026 | { |
1020 | if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) | 1027 | if (m_taintAngularLock != 0) |
1021 | { | 1028 | { |
1022 | //d.BodySetFiniteRotationMode(Body, 0); | ||
1023 | //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); | ||
1024 | createAMotor(m_taintAngularLock); | 1029 | createAMotor(m_taintAngularLock); |
1025 | } | 1030 | } |
1026 | else | 1031 | else |
@@ -1034,7 +1039,6 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1034 | } | 1039 | } |
1035 | } | 1040 | } |
1036 | 1041 | ||
1037 | // Store this for later in case we get turned into a separate body | ||
1038 | m_angularlock = m_taintAngularLock; | 1042 | m_angularlock = m_taintAngularLock; |
1039 | } | 1043 | } |
1040 | 1044 | ||
@@ -1070,7 +1074,7 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1070 | else if (_parent != null && m_taintparent == null) | 1074 | else if (_parent != null && m_taintparent == null) |
1071 | { | 1075 | { |
1072 | //Console.WriteLine(" changelink B"); | 1076 | //Console.WriteLine(" changelink B"); |
1073 | 1077 | ||
1074 | if (_parent is OdePrim) | 1078 | if (_parent is OdePrim) |
1075 | { | 1079 | { |
1076 | OdePrim obj = (OdePrim)_parent; | 1080 | OdePrim obj = (OdePrim)_parent; |
@@ -1078,16 +1082,16 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1078 | childPrim = false; | 1082 | childPrim = false; |
1079 | //_parent = null; | 1083 | //_parent = null; |
1080 | } | 1084 | } |
1081 | 1085 | ||
1082 | /* | 1086 | /* |
1083 | if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) | 1087 | if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) |
1084 | d.JointGroupDestroy(_linkJointGroup); | 1088 | d.JointGroupDestroy(_linkJointGroup); |
1085 | 1089 | ||
1086 | _linkJointGroup = (IntPtr)0; | 1090 | _linkJointGroup = (IntPtr)0; |
1087 | m_linkJoint = (IntPtr)0; | 1091 | m_linkJoint = (IntPtr)0; |
1088 | */ | 1092 | */ |
1089 | } | 1093 | } |
1090 | 1094 | ||
1091 | _parent = m_taintparent; | 1095 | _parent = m_taintparent; |
1092 | m_taintPhysics = IsPhysical; | 1096 | m_taintPhysics = IsPhysical; |
1093 | } | 1097 | } |
@@ -1145,12 +1149,12 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1145 | if (prm.m_assetFailed) | 1149 | if (prm.m_assetFailed) |
1146 | { | 1150 | { |
1147 | d.GeomSetCategoryBits(prm.prim_geom, 0); | 1151 | d.GeomSetCategoryBits(prm.prim_geom, 0); |
1148 | d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits); | 1152 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.BadMeshAssetCollideBits); |
1149 | } | 1153 | } |
1150 | else | 1154 | else |
1151 | { | 1155 | { |
1152 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); | 1156 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); |
1153 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); | 1157 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); |
1154 | } | 1158 | } |
1155 | 1159 | ||
1156 | d.Quaternion quat = new d.Quaternion(); | 1160 | d.Quaternion quat = new d.Quaternion(); |
@@ -1194,14 +1198,14 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1194 | if (m_assetFailed) | 1198 | if (m_assetFailed) |
1195 | { | 1199 | { |
1196 | d.GeomSetCategoryBits(prim_geom, 0); | 1200 | d.GeomSetCategoryBits(prim_geom, 0); |
1197 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | 1201 | d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); |
1198 | } | 1202 | } |
1199 | else | 1203 | else |
1200 | { | 1204 | { |
1201 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); | 1205 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); |
1202 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 1206 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
1203 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); | 1207 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); |
1204 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 1208 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
1205 | } | 1209 | } |
1206 | 1210 | ||
1207 | d.Quaternion quat2 = new d.Quaternion(); | 1211 | d.Quaternion quat2 = new d.Quaternion(); |
@@ -1230,7 +1234,8 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1230 | m_disabled = false; | 1234 | m_disabled = false; |
1231 | 1235 | ||
1232 | // The body doesn't already have a finite rotation mode set here | 1236 | // The body doesn't already have a finite rotation mode set here |
1233 | if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null) | 1237 | // or remove |
1238 | if (_parent == null) | ||
1234 | { | 1239 | { |
1235 | createAMotor(m_angularlock); | 1240 | createAMotor(m_angularlock); |
1236 | } | 1241 | } |
@@ -1340,7 +1345,7 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1340 | // in between the disabling and the collision properties setting | 1345 | // in between the disabling and the collision properties setting |
1341 | // which would wake the physical body up from a soft disabling and potentially cause it to fall | 1346 | // which would wake the physical body up from a soft disabling and potentially cause it to fall |
1342 | // through the ground. | 1347 | // through the ground. |
1343 | 1348 | ||
1344 | // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select | 1349 | // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select |
1345 | // just one part of the assembly, the rest of the assembly is non-selected and still simulating, | 1350 | // just one part of the assembly, the rest of the assembly is non-selected and still simulating, |
1346 | // so that causes the selected part to wake up and continue moving. | 1351 | // so that causes the selected part to wake up and continue moving. |
@@ -1354,7 +1359,7 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1354 | // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are | 1359 | // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are |
1355 | // selected and disabled. then, due to a thread switch, the selection processing is | 1360 | // selected and disabled. then, due to a thread switch, the selection processing is |
1356 | // interrupted and the physics engine continues to simulate, so the last 50 items, whose | 1361 | // interrupted and the physics engine continues to simulate, so the last 50 items, whose |
1357 | // selection was not yet processed, continues to simulate. this wakes up ALL of the | 1362 | // selection was not yet processed, continues to simulate. this wakes up ALL of the |
1358 | // first 50 again. then the last 50 are disabled. then the first 50, which were just woken | 1363 | // first 50 again. then the last 50 are disabled. then the first 50, which were just woken |
1359 | // up, start simulating again, which in turn wakes up the last 50. | 1364 | // up, start simulating again, which in turn wakes up the last 50. |
1360 | 1365 | ||
@@ -1370,8 +1375,8 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1370 | } | 1375 | } |
1371 | else | 1376 | else |
1372 | { | 1377 | { |
1373 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 1378 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
1374 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 1379 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
1375 | } | 1380 | } |
1376 | 1381 | ||
1377 | if (IsPhysical) | 1382 | if (IsPhysical) |
@@ -1396,12 +1401,12 @@ Console.WriteLine("ZProcessTaints for " + Name); | |||
1396 | if (m_assetFailed) | 1401 | if (m_assetFailed) |
1397 | { | 1402 | { |
1398 | d.GeomSetCategoryBits(prim_geom, 0); | 1403 | d.GeomSetCategoryBits(prim_geom, 0); |
1399 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | 1404 | d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); |
1400 | } | 1405 | } |
1401 | else | 1406 | else |
1402 | { | 1407 | { |
1403 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 1408 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
1404 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 1409 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
1405 | } | 1410 | } |
1406 | 1411 | ||
1407 | if (IsPhysical) | 1412 | if (IsPhysical) |
@@ -1560,7 +1565,7 @@ Console.WriteLine("CreateGeom:"); | |||
1560 | private void changeadd() | 1565 | private void changeadd() |
1561 | { | 1566 | { |
1562 | // m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); | 1567 | // m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); |
1563 | 1568 | ||
1564 | int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | 1569 | int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); |
1565 | IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); | 1570 | IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); |
1566 | 1571 | ||
@@ -1686,6 +1691,9 @@ Console.WriteLine(" JointCreateFixed"); | |||
1686 | float fy = 0; | 1691 | float fy = 0; |
1687 | float fz = 0; | 1692 | float fz = 0; |
1688 | 1693 | ||
1694 | if (outofBounds) | ||
1695 | return; | ||
1696 | |||
1689 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. | 1697 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. |
1690 | { | 1698 | { |
1691 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 1699 | if (m_vehicle.Type != Vehicle.TYPE_NONE) |
@@ -1697,31 +1705,16 @@ Console.WriteLine(" JointCreateFixed"); | |||
1697 | { | 1705 | { |
1698 | //Console.WriteLine("Move " + Name); | 1706 | //Console.WriteLine("Move " + Name); |
1699 | if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 | 1707 | if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 |
1700 | // NON-'VEHICLES' are dealt with here | ||
1701 | // if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) | ||
1702 | // { | ||
1703 | // d.Vector3 avel2 = d.BodyGetAngularVel(Body); | ||
1704 | // /* | ||
1705 | // if (m_angularlock.X == 1) | ||
1706 | // avel2.X = 0; | ||
1707 | // if (m_angularlock.Y == 1) | ||
1708 | // avel2.Y = 0; | ||
1709 | // if (m_angularlock.Z == 1) | ||
1710 | // avel2.Z = 0; | ||
1711 | // d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); | ||
1712 | // */ | ||
1713 | // } | ||
1714 | //float PID_P = 900.0f; | ||
1715 | 1708 | ||
1716 | float m_mass = CalculateMass(); | 1709 | float m_mass = CalculateMass(); |
1717 | 1710 | ||
1718 | // fz = 0f; | 1711 | // fz = 0f; |
1719 | //m_log.Info(m_collisionFlags.ToString()); | 1712 | //m_log.Info(m_collisionFlags.ToString()); |
1720 | 1713 | ||
1721 | 1714 | ||
1722 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 1715 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. |
1723 | // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? | 1716 | // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? |
1724 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up | 1717 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up |
1725 | // gravityz multiplier = 1 - m_buoyancy | 1718 | // gravityz multiplier = 1 - m_buoyancy |
1726 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; | 1719 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; |
1727 | 1720 | ||
@@ -1736,7 +1729,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1736 | fz = 0f; | 1729 | fz = 0f; |
1737 | 1730 | ||
1738 | // no lock; for now it's only called from within Simulate() | 1731 | // no lock; for now it's only called from within Simulate() |
1739 | 1732 | ||
1740 | // If the PID Controller isn't active then we set our force | 1733 | // If the PID Controller isn't active then we set our force |
1741 | // calculating base velocity to the current position | 1734 | // calculating base velocity to the current position |
1742 | 1735 | ||
@@ -1745,7 +1738,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1745 | //PID_G = PID_G / m_PIDTau; | 1738 | //PID_G = PID_G / m_PIDTau; |
1746 | m_PIDTau = 1; | 1739 | m_PIDTau = 1; |
1747 | } | 1740 | } |
1748 | 1741 | ||
1749 | if ((PID_G - m_PIDTau) <= 0) | 1742 | if ((PID_G - m_PIDTau) <= 0) |
1750 | { | 1743 | { |
1751 | PID_G = m_PIDTau + 1; | 1744 | PID_G = m_PIDTau + 1; |
@@ -1768,7 +1761,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1768 | if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) | 1761 | if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) |
1769 | { | 1762 | { |
1770 | // keep track of where we stopped. No more slippin' & slidin' | 1763 | // keep track of where we stopped. No more slippin' & slidin' |
1771 | 1764 | ||
1772 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | 1765 | // We only want to deactivate the PID Controller if we think we want to have our surrogate |
1773 | // react to the physics scene by moving it's position. | 1766 | // react to the physics scene by moving it's position. |
1774 | // Avatar to Avatar collisions | 1767 | // Avatar to Avatar collisions |
@@ -1789,7 +1782,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1789 | // We're flying and colliding with something | 1782 | // We're flying and colliding with something |
1790 | fx = ((_target_velocity.X) - vel.X) * (PID_D); | 1783 | fx = ((_target_velocity.X) - vel.X) * (PID_D); |
1791 | fy = ((_target_velocity.Y) - vel.Y) * (PID_D); | 1784 | fy = ((_target_velocity.Y) - vel.Y) * (PID_D); |
1792 | 1785 | ||
1793 | // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; | 1786 | // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; |
1794 | 1787 | ||
1795 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | 1788 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); |
@@ -1800,7 +1793,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1800 | if (m_useHoverPID && !PIDActive) | 1793 | if (m_useHoverPID && !PIDActive) |
1801 | { | 1794 | { |
1802 | //Console.WriteLine("Hover " + Name); | 1795 | //Console.WriteLine("Hover " + Name); |
1803 | 1796 | ||
1804 | // If we're using the PID controller, then we have no gravity | 1797 | // If we're using the PID controller, then we have no gravity |
1805 | fz = (-1 * _parent_scene.gravityz) * m_mass; | 1798 | fz = (-1 * _parent_scene.gravityz) * m_mass; |
1806 | 1799 | ||
@@ -1857,7 +1850,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1857 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) | 1850 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) |
1858 | { | 1851 | { |
1859 | // keep track of where we stopped. No more slippin' & slidin' | 1852 | // keep track of where we stopped. No more slippin' & slidin' |
1860 | 1853 | ||
1861 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | 1854 | // We only want to deactivate the PID Controller if we think we want to have our surrogate |
1862 | // react to the physics scene by moving it's position. | 1855 | // react to the physics scene by moving it's position. |
1863 | // Avatar to Avatar collisions | 1856 | // Avatar to Avatar collisions |
@@ -1895,7 +1888,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1895 | { | 1888 | { |
1896 | // A physical body at rest on a surface will auto-disable after a while, | 1889 | // A physical body at rest on a surface will auto-disable after a while, |
1897 | // this appears to re-enable it incase the surface it is upon vanishes, | 1890 | // this appears to re-enable it incase the surface it is upon vanishes, |
1898 | // and the body should fall again. | 1891 | // and the body should fall again. |
1899 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | 1892 | d.BodySetLinearVel(Body, 0f, 0f, 0f); |
1900 | d.BodySetForce(Body, 0, 0, 0); | 1893 | d.BodySetForce(Body, 0, 0, 0); |
1901 | enableBodySoft(); | 1894 | enableBodySoft(); |
@@ -1904,7 +1897,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1904 | // 35x10 = 350n times the mass per second applied maximum. | 1897 | // 35x10 = 350n times the mass per second applied maximum. |
1905 | float nmax = 35f * m_mass; | 1898 | float nmax = 35f * m_mass; |
1906 | float nmin = -35f * m_mass; | 1899 | float nmin = -35f * m_mass; |
1907 | 1900 | ||
1908 | if (fx > nmax) | 1901 | if (fx > nmax) |
1909 | fx = nmax; | 1902 | fx = nmax; |
1910 | if (fx < nmin) | 1903 | if (fx < nmin) |
@@ -1923,7 +1916,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1923 | // _zeroPosition = d.BodyGetPosition(Body); | 1916 | // _zeroPosition = d.BodyGetPosition(Body); |
1924 | return; | 1917 | return; |
1925 | //Console.WriteLine("Nothing " + Name); | 1918 | //Console.WriteLine("Nothing " + Name); |
1926 | 1919 | ||
1927 | } | 1920 | } |
1928 | } | 1921 | } |
1929 | 1922 | ||
@@ -1940,8 +1933,8 @@ Console.WriteLine(" JointCreateFixed"); | |||
1940 | d.BodySetQuaternion(Body, ref myrot); | 1933 | d.BodySetQuaternion(Body, ref myrot); |
1941 | if (IsPhysical) | 1934 | if (IsPhysical) |
1942 | { | 1935 | { |
1943 | if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) | 1936 | // create or remove locks |
1944 | createAMotor(m_angularlock); | 1937 | createAMotor(m_angularlock); |
1945 | } | 1938 | } |
1946 | } | 1939 | } |
1947 | else | 1940 | else |
@@ -1949,7 +1942,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1949 | // daughter prim, do Geom set | 1942 | // daughter prim, do Geom set |
1950 | d.GeomSetQuaternion(prim_geom, ref myrot); | 1943 | d.GeomSetQuaternion(prim_geom, ref myrot); |
1951 | } | 1944 | } |
1952 | 1945 | ||
1953 | resetCollisionAccounting(); | 1946 | resetCollisionAccounting(); |
1954 | m_taintrot = _orientation; | 1947 | m_taintrot = _orientation; |
1955 | } | 1948 | } |
@@ -2087,7 +2080,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2087 | else | 2080 | else |
2088 | m_assetFailed = false; | 2081 | m_assetFailed = false; |
2089 | } | 2082 | } |
2090 | 2083 | ||
2091 | } | 2084 | } |
2092 | 2085 | ||
2093 | CreateGeom(m_targetSpace, mesh); | 2086 | CreateGeom(m_targetSpace, mesh); |
@@ -2140,10 +2133,10 @@ Console.WriteLine(" JointCreateFixed"); | |||
2140 | } | 2133 | } |
2141 | 2134 | ||
2142 | if (m_assetFailed) | 2135 | if (m_assetFailed) |
2143 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | 2136 | d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); |
2144 | else | 2137 | else |
2145 | 2138 | ||
2146 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 2139 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
2147 | } | 2140 | } |
2148 | /// <summary> | 2141 | /// <summary> |
2149 | /// Change prim in response to a shape taint. | 2142 | /// Change prim in response to a shape taint. |
@@ -2319,7 +2312,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2319 | } | 2312 | } |
2320 | d.BodyEnable(Body); | 2313 | d.BodyEnable(Body); |
2321 | d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); | 2314 | d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); |
2322 | 2315 | ||
2323 | } | 2316 | } |
2324 | m_angularforcelist.Clear(); | 2317 | m_angularforcelist.Clear(); |
2325 | } | 2318 | } |
@@ -2349,7 +2342,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2349 | d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); | 2342 | d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); |
2350 | } | 2343 | } |
2351 | } | 2344 | } |
2352 | 2345 | ||
2353 | //resetCollisionAccounting(); | 2346 | //resetCollisionAccounting(); |
2354 | } | 2347 | } |
2355 | 2348 | ||
@@ -2659,16 +2652,46 @@ Console.WriteLine(" JointCreateFixed"); | |||
2659 | 2652 | ||
2660 | public override void CrossingFailure() | 2653 | public override void CrossingFailure() |
2661 | { | 2654 | { |
2662 | m_crossingfailures++; | 2655 | /* |
2663 | if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) | 2656 | m_crossingfailures++; |
2664 | { | 2657 | if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) |
2665 | base.RaiseOutOfBounds(_position); | 2658 | { |
2666 | return; | 2659 | base.RaiseOutOfBounds(_position); |
2667 | } | 2660 | return; |
2668 | else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) | 2661 | } |
2662 | else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2663 | { | ||
2664 | m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); | ||
2665 | } | ||
2666 | */ | ||
2667 | |||
2668 | d.AllocateODEDataForThread(0U); | ||
2669 | |||
2670 | _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); | ||
2671 | _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); | ||
2672 | _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); | ||
2673 | |||
2674 | m_lastposition = _position; | ||
2675 | _velocity.X = 0; | ||
2676 | _velocity.Y = 0; | ||
2677 | _velocity.Z = 0; | ||
2678 | |||
2679 | m_lastVelocity = _velocity; | ||
2680 | |||
2681 | if (Body != IntPtr.Zero) | ||
2669 | { | 2682 | { |
2670 | m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); | 2683 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it |
2684 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
2671 | } | 2685 | } |
2686 | |||
2687 | if(m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
2688 | m_vehicle.Stop(); // this also updates vehicle last position from the body position | ||
2689 | |||
2690 | enableBodySoft(); | ||
2691 | |||
2692 | outofBounds = false; | ||
2693 | base.RequestPhysicsterseUpdate(); | ||
2694 | |||
2672 | } | 2695 | } |
2673 | 2696 | ||
2674 | public override float Buoyancy | 2697 | public override float Buoyancy |
@@ -2687,32 +2710,23 @@ Console.WriteLine(" JointCreateFixed"); | |||
2687 | m_taintparent = null; | 2710 | m_taintparent = null; |
2688 | } | 2711 | } |
2689 | 2712 | ||
2690 | public override void LockAngularMotion(Vector3 axis) | 2713 | public override void LockAngularMotion(byte axislocks) |
2691 | { | 2714 | { |
2692 | // reverse the zero/non zero values for ODE. | 2715 | // m_log.DebugFormat("[axislocks]: {0}", axislocks); |
2693 | if (axis.IsFinite()) | 2716 | m_taintAngularLock = axislocks; |
2694 | { | ||
2695 | axis.X = (axis.X > 0) ? 1f : 0f; | ||
2696 | axis.Y = (axis.Y > 0) ? 1f : 0f; | ||
2697 | axis.Z = (axis.Z > 0) ? 1f : 0f; | ||
2698 | m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | ||
2699 | m_taintAngularLock = axis; | ||
2700 | } | ||
2701 | else | ||
2702 | { | ||
2703 | m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); | ||
2704 | } | ||
2705 | } | 2717 | } |
2706 | 2718 | ||
2707 | internal void UpdatePositionAndVelocity() | 2719 | internal void UpdatePositionAndVelocity() |
2708 | { | 2720 | { |
2709 | // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! | 2721 | // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! |
2722 | if (outofBounds) | ||
2723 | return; | ||
2710 | if (_parent == null) | 2724 | if (_parent == null) |
2711 | { | 2725 | { |
2712 | Vector3 pv = Vector3.Zero; | 2726 | Vector3 pv = Vector3.Zero; |
2713 | bool lastZeroFlag = _zeroFlag; | 2727 | bool lastZeroFlag = _zeroFlag; |
2714 | float m_minvelocity = 0; | 2728 | float m_minvelocity = 0; |
2715 | if (Body != (IntPtr)0) // FIXME -> or if it is a joint | 2729 | if (Body != IntPtr.Zero) // FIXME -> or if it is a joint |
2716 | { | 2730 | { |
2717 | d.Vector3 vec = d.BodyGetPosition(Body); | 2731 | d.Vector3 vec = d.BodyGetPosition(Body); |
2718 | d.Quaternion ori = d.BodyGetQuaternion(Body); | 2732 | d.Quaternion ori = d.BodyGetQuaternion(Body); |
@@ -2723,12 +2737,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
2723 | Vector3 l_position = Vector3.Zero; | 2737 | Vector3 l_position = Vector3.Zero; |
2724 | Quaternion l_orientation = Quaternion.Identity; | 2738 | Quaternion l_orientation = Quaternion.Identity; |
2725 | 2739 | ||
2726 | // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) | ||
2727 | //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | ||
2728 | //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | ||
2729 | //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | ||
2730 | //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | ||
2731 | |||
2732 | m_lastposition = _position; | 2740 | m_lastposition = _position; |
2733 | m_lastorientation = _orientation; | 2741 | m_lastorientation = _orientation; |
2734 | 2742 | ||
@@ -2740,26 +2748,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
2740 | l_orientation.Z = ori.Z; | 2748 | l_orientation.Z = ori.Z; |
2741 | l_orientation.W = ori.W; | 2749 | l_orientation.W = ori.W; |
2742 | 2750 | ||
2743 | 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) | ||
2744 | { | ||
2745 | //base.RaiseOutOfBounds(l_position); | ||
2746 | |||
2747 | if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2748 | { | ||
2749 | _position = l_position; | ||
2750 | //_parent_scene.remActivePrim(this); | ||
2751 | if (_parent == null) | ||
2752 | base.RequestPhysicsterseUpdate(); | ||
2753 | return; | ||
2754 | } | ||
2755 | else | ||
2756 | { | ||
2757 | if (_parent == null) | ||
2758 | base.RaiseOutOfBounds(l_position); | ||
2759 | return; | ||
2760 | } | ||
2761 | } | ||
2762 | |||
2763 | if (l_position.Z < 0) | 2751 | if (l_position.Z < 0) |
2764 | { | 2752 | { |
2765 | // This is so prim that get lost underground don't fall forever and suck up | 2753 | // This is so prim that get lost underground don't fall forever and suck up |
@@ -2769,8 +2757,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
2769 | // It's a hack and will generate a console message if it fails. | 2757 | // It's a hack and will generate a console message if it fails. |
2770 | 2758 | ||
2771 | //IsPhysical = false; | 2759 | //IsPhysical = false; |
2772 | if (_parent == null) | ||
2773 | base.RaiseOutOfBounds(_position); | ||
2774 | 2760 | ||
2775 | _acceleration.X = 0; | 2761 | _acceleration.X = 0; |
2776 | _acceleration.Y = 0; | 2762 | _acceleration.Y = 0; |
@@ -2784,16 +2770,65 @@ Console.WriteLine(" JointCreateFixed"); | |||
2784 | m_rotationalVelocity.Z = 0; | 2770 | m_rotationalVelocity.Z = 0; |
2785 | 2771 | ||
2786 | if (_parent == null) | 2772 | if (_parent == null) |
2773 | base.RaiseOutOfBounds(_position); | ||
2774 | |||
2775 | if (_parent == null) | ||
2787 | base.RequestPhysicsterseUpdate(); | 2776 | base.RequestPhysicsterseUpdate(); |
2788 | 2777 | ||
2789 | m_throttleUpdates = false; | 2778 | m_throttleUpdates = false; |
2790 | throttleCounter = 0; | 2779 | throttleCounter = 0; |
2791 | _zeroFlag = true; | 2780 | _zeroFlag = true; |
2792 | //outofBounds = true; | 2781 | //outofBounds = true; |
2782 | return; | ||
2783 | } | ||
2784 | |||
2785 | 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) | ||
2786 | { | ||
2787 | //base.RaiseOutOfBounds(l_position); | ||
2788 | /* | ||
2789 | if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2790 | { | ||
2791 | _position = l_position; | ||
2792 | //_parent_scene.remActivePrim(this); | ||
2793 | if (_parent == null) | ||
2794 | base.RequestPhysicsterseUpdate(); | ||
2795 | return; | ||
2796 | } | ||
2797 | else | ||
2798 | { | ||
2799 | if (_parent == null) | ||
2800 | base.RaiseOutOfBounds(l_position); | ||
2801 | return; | ||
2802 | } | ||
2803 | */ | ||
2804 | outofBounds = true; | ||
2805 | // part near the border on outside | ||
2806 | if (l_position.X < 0) | ||
2807 | Util.Clamp(l_position.X, -0.1f, -2f); | ||
2808 | else | ||
2809 | Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); | ||
2810 | if (l_position.Y < 0) | ||
2811 | Util.Clamp(l_position.Y, -0.1f, -2f); | ||
2812 | else | ||
2813 | Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); | ||
2814 | |||
2815 | d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z); | ||
2816 | |||
2817 | // stop it | ||
2818 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
2819 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
2820 | disableBodySoft(); | ||
2821 | |||
2822 | _position = l_position; | ||
2823 | // tell framework to fix it | ||
2824 | if (_parent == null) | ||
2825 | base.RequestPhysicsterseUpdate(); | ||
2826 | return; | ||
2793 | } | 2827 | } |
2794 | 2828 | ||
2829 | |||
2795 | //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); | 2830 | //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); |
2796 | //Console.WriteLine("Adiff " + Name + " = " + Adiff); | 2831 | //Console.WriteLine("Adiff " + Name + " = " + Adiff); |
2797 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) | 2832 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) |
2798 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) | 2833 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) |
2799 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) | 2834 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) |
@@ -2864,12 +2899,12 @@ Console.WriteLine(" JointCreateFixed"); | |||
2864 | _acceleration = ((_velocity - m_lastVelocity) / 0.1f); | 2899 | _acceleration = ((_velocity - m_lastVelocity) / 0.1f); |
2865 | _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); | 2900 | _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); |
2866 | //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); | 2901 | //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); |
2867 | 2902 | ||
2868 | // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... | 2903 | // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... |
2869 | // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. | 2904 | // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. |
2870 | // 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 | 2905 | // 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 |
2871 | // adding these logical exclusion situations to maintain this where I think it was intended to be. | 2906 | // adding these logical exclusion situations to maintain this where I think it was intended to be. |
2872 | if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) | 2907 | if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) |
2873 | { | 2908 | { |
2874 | m_minvelocity = 0.5f; | 2909 | m_minvelocity = 0.5f; |
2875 | } | 2910 | } |
@@ -2938,8 +2973,8 @@ Console.WriteLine(" JointCreateFixed"); | |||
2938 | { | 2973 | { |
2939 | } | 2974 | } |
2940 | 2975 | ||
2941 | public override Vector3 PIDTarget | 2976 | public override Vector3 PIDTarget |
2942 | { | 2977 | { |
2943 | set | 2978 | set |
2944 | { | 2979 | { |
2945 | if (value.IsFinite()) | 2980 | if (value.IsFinite()) |
@@ -2948,16 +2983,17 @@ Console.WriteLine(" JointCreateFixed"); | |||
2948 | } | 2983 | } |
2949 | else | 2984 | else |
2950 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); | 2985 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); |
2951 | } | 2986 | } |
2952 | } | 2987 | } |
2988 | |||
2953 | public override bool PIDActive { get; set; } | 2989 | public override bool PIDActive { get; set; } |
2954 | public override float PIDTau { set { m_PIDTau = value; } } | 2990 | public override float PIDTau { set { m_PIDTau = value; } } |
2955 | 2991 | ||
2956 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } | 2992 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } |
2957 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } | 2993 | public override bool PIDHoverActive { get { return m_useHoverPID;} set { m_useHoverPID = value; } } |
2958 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } | 2994 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } |
2959 | public override float PIDHoverTau { set { m_PIDHoverTau = value; } } | 2995 | public override float PIDHoverTau { set { m_PIDHoverTau = value; } } |
2960 | 2996 | ||
2961 | public override Quaternion APIDTarget{ set { return; } } | 2997 | public override Quaternion APIDTarget{ set { return; } } |
2962 | 2998 | ||
2963 | public override bool APIDActive{ set { return; } } | 2999 | public override bool APIDActive{ set { return; } } |
@@ -2966,7 +3002,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2966 | 3002 | ||
2967 | public override float APIDDamping{ set { return; } } | 3003 | public override float APIDDamping{ set { return; } } |
2968 | 3004 | ||
2969 | private void createAMotor(Vector3 axis) | 3005 | private void createAMotor(byte axislock) |
2970 | { | 3006 | { |
2971 | if (Body == IntPtr.Zero) | 3007 | if (Body == IntPtr.Zero) |
2972 | return; | 3008 | return; |
@@ -2977,150 +3013,105 @@ Console.WriteLine(" JointCreateFixed"); | |||
2977 | Amotor = IntPtr.Zero; | 3013 | Amotor = IntPtr.Zero; |
2978 | } | 3014 | } |
2979 | 3015 | ||
2980 | float axisnum = 3; | 3016 | if(axislock == 0) |
2981 | 3017 | return; | |
2982 | axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); | ||
2983 | |||
2984 | // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); | ||
2985 | |||
2986 | |||
2987 | // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. | ||
2988 | d.Mass objMass; | ||
2989 | d.MassSetZero(out objMass); | ||
2990 | DMassCopy(ref pMass, ref objMass); | ||
2991 | |||
2992 | //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); | ||
2993 | |||
2994 | Matrix4 dMassMat = FromDMass(objMass); | ||
2995 | |||
2996 | Matrix4 mathmat = Inverse(dMassMat); | ||
2997 | |||
2998 | /* | ||
2999 | //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]); | ||
3000 | |||
3001 | mathmat = Inverse(mathmat); | ||
3002 | |||
3003 | |||
3004 | objMass = FromMatrix4(mathmat, ref objMass); | ||
3005 | //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); | ||
3006 | |||
3007 | mathmat = Inverse(mathmat); | ||
3008 | */ | ||
3009 | if (axis.X == 0) | ||
3010 | { | ||
3011 | mathmat.M33 = 50.0000001f; | ||
3012 | //objMass.I.M22 = 0; | ||
3013 | } | ||
3014 | if (axis.Y == 0) | ||
3015 | { | ||
3016 | mathmat.M22 = 50.0000001f; | ||
3017 | //objMass.I.M11 = 0; | ||
3018 | } | ||
3019 | if (axis.Z == 0) | ||
3020 | { | ||
3021 | mathmat.M11 = 50.0000001f; | ||
3022 | //objMass.I.M00 = 0; | ||
3023 | } | ||
3024 | |||
3025 | |||
3026 | 3018 | ||
3027 | mathmat = Inverse(mathmat); | 3019 | int axisnum = 0; |
3028 | objMass = FromMatrix4(mathmat, ref objMass); | 3020 | bool axisX = false; |
3029 | //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); | 3021 | bool axisY = false; |
3030 | 3022 | bool axisZ = false; | |
3031 | //return; | 3023 | if((axislock & 0x02) != 0) |
3032 | if (d.MassCheck(ref objMass)) | 3024 | { |
3033 | { | 3025 | axisnum++; |
3034 | d.BodySetMass(Body, ref objMass); | 3026 | axisX = true; |
3035 | } | 3027 | } |
3036 | else | 3028 | if((axislock & 0x04) != 0) |
3037 | { | 3029 | { |
3038 | //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); | 3030 | axisnum++; |
3039 | } | 3031 | axisY = true; |
3032 | } | ||
3033 | if((axislock & 0x08) != 0) | ||
3034 | { | ||
3035 | axisnum++; | ||
3036 | axisZ = true; | ||
3037 | } | ||
3040 | 3038 | ||
3041 | if (axisnum <= 0) | 3039 | if(axisnum == 0) |
3042 | return; | 3040 | return; |
3043 | // int dAMotorEuler = 1; | 3041 | // stop it |
3042 | d.BodySetTorque(Body, 0, 0, 0); | ||
3043 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
3044 | 3044 | ||
3045 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | 3045 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); |
3046 | d.JointAttach(Amotor, Body, IntPtr.Zero); | 3046 | d.JointAttach(Amotor, Body, IntPtr.Zero); |
3047 | |||
3047 | d.JointSetAMotorMode(Amotor, 0); | 3048 | d.JointSetAMotorMode(Amotor, 0); |
3048 | 3049 | ||
3049 | d.JointSetAMotorNumAxes(Amotor,(int)axisnum); | 3050 | d.JointSetAMotorNumAxes(Amotor, axisnum); |
3050 | int i = 0; | ||
3051 | 3051 | ||
3052 | if (axis.X == 0) | 3052 | // get current orientation to lock |
3053 | { | ||
3054 | d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); | ||
3055 | i++; | ||
3056 | } | ||
3057 | 3053 | ||
3058 | if (axis.Y == 0) | 3054 | d.Quaternion dcur = d.BodyGetQuaternion(Body); |
3059 | { | 3055 | Quaternion curr; // crap convertion between identical things |
3060 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); | 3056 | curr.X = dcur.X; |
3061 | i++; | 3057 | curr.Y = dcur.Y; |
3062 | } | 3058 | curr.Z = dcur.Z; |
3059 | curr.W = dcur.W; | ||
3060 | Vector3 ax; | ||
3063 | 3061 | ||
3064 | if (axis.Z == 0) | 3062 | int i = 0; |
3065 | { | 3063 | int j = 0; |
3066 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); | 3064 | if (axisX) |
3065 | { | ||
3066 | ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X | ||
3067 | d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); | ||
3068 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
3069 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); | ||
3070 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); | ||
3071 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
3072 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); | ||
3073 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); | ||
3074 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f); | ||
3075 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); | ||
3076 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); | ||
3077 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); | ||
3078 | i++; | ||
3079 | j = 256; // move to next axis set | ||
3080 | } | ||
3081 | |||
3082 | if (axisY) | ||
3083 | { | ||
3084 | ax = (new Vector3(0, 1, 0)) * curr; | ||
3085 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
3086 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
3087 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
3088 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
3089 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
3090 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
3091 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
3092 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f); | ||
3093 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
3094 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
3095 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
3067 | i++; | 3096 | i++; |
3097 | j += 256; | ||
3068 | } | 3098 | } |
3069 | 3099 | ||
3070 | for (int j = 0; j < (int)axisnum; j++) | 3100 | if (axisZ) |
3071 | { | 3101 | { |
3072 | //d.JointSetAMotorAngle(Amotor, j, 0); | 3102 | ax = (new Vector3(0, 0, 1)) * curr; |
3103 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
3104 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
3105 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
3106 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
3107 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
3108 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
3109 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
3110 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f); | ||
3111 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
3112 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
3113 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
3073 | } | 3114 | } |
3074 | |||
3075 | //d.JointSetAMotorAngle(Amotor, 1, 0); | ||
3076 | //d.JointSetAMotorAngle(Amotor, 2, 0); | ||
3077 | |||
3078 | // These lowstops and high stops are effectively (no wiggle room) | ||
3079 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); | ||
3080 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); | ||
3081 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); | ||
3082 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); | ||
3083 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); | ||
3084 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); | ||
3085 | //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); | ||
3086 | d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); | ||
3087 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// | ||
3088 | } | ||
3089 | |||
3090 | private Matrix4 FromDMass(d.Mass pMass) | ||
3091 | { | ||
3092 | Matrix4 obj; | ||
3093 | obj.M11 = pMass.I.M00; | ||
3094 | obj.M12 = pMass.I.M01; | ||
3095 | obj.M13 = pMass.I.M02; | ||
3096 | obj.M14 = 0; | ||
3097 | obj.M21 = pMass.I.M10; | ||
3098 | obj.M22 = pMass.I.M11; | ||
3099 | obj.M23 = pMass.I.M12; | ||
3100 | obj.M24 = 0; | ||
3101 | obj.M31 = pMass.I.M20; | ||
3102 | obj.M32 = pMass.I.M21; | ||
3103 | obj.M33 = pMass.I.M22; | ||
3104 | obj.M34 = 0; | ||
3105 | obj.M41 = 0; | ||
3106 | obj.M42 = 0; | ||
3107 | obj.M43 = 0; | ||
3108 | obj.M44 = 1; | ||
3109 | return obj; | ||
3110 | } | ||
3111 | |||
3112 | private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) | ||
3113 | { | ||
3114 | obj.I.M00 = pMat[0, 0]; | ||
3115 | obj.I.M01 = pMat[0, 1]; | ||
3116 | obj.I.M02 = pMat[0, 2]; | ||
3117 | obj.I.M10 = pMat[1, 0]; | ||
3118 | obj.I.M11 = pMat[1, 1]; | ||
3119 | obj.I.M12 = pMat[1, 2]; | ||
3120 | obj.I.M20 = pMat[2, 0]; | ||
3121 | obj.I.M21 = pMat[2, 1]; | ||
3122 | obj.I.M22 = pMat[2, 2]; | ||
3123 | return obj; | ||
3124 | } | 3115 | } |
3125 | 3116 | ||
3126 | public override void SubscribeEvents(int ms) | 3117 | public override void SubscribeEvents(int ms) |
@@ -3135,7 +3126,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
3135 | m_eventsubscription = 0; | 3126 | m_eventsubscription = 0; |
3136 | } | 3127 | } |
3137 | 3128 | ||
3138 | public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | 3129 | public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) |
3139 | { | 3130 | { |
3140 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | 3131 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); |
3141 | } | 3132 | } |
@@ -3165,173 +3156,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
3165 | return false; | 3156 | return false; |
3166 | } | 3157 | } |
3167 | 3158 | ||
3168 | public static Matrix4 Inverse(Matrix4 pMat) | ||
3169 | { | ||
3170 | if (determinant3x3(pMat) == 0) | ||
3171 | { | ||
3172 | return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible | ||
3173 | } | ||
3174 | |||
3175 | return (Adjoint(pMat) / determinant3x3(pMat)); | ||
3176 | } | ||
3177 | |||
3178 | public static Matrix4 Adjoint(Matrix4 pMat) | ||
3179 | { | ||
3180 | Matrix4 adjointMatrix = new Matrix4(); | ||
3181 | for (int i=0; i<4; i++) | ||
3182 | { | ||
3183 | for (int j=0; j<4; j++) | ||
3184 | { | ||
3185 | Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); | ||
3186 | } | ||
3187 | } | ||
3188 | |||
3189 | adjointMatrix = Transpose(adjointMatrix); | ||
3190 | return adjointMatrix; | ||
3191 | } | ||
3192 | |||
3193 | public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) | ||
3194 | { | ||
3195 | Matrix4 minor = new Matrix4(); | ||
3196 | int m = 0, n = 0; | ||
3197 | for (int i = 0; i < 4; i++) | ||
3198 | { | ||
3199 | if (i == iRow) | ||
3200 | continue; | ||
3201 | n = 0; | ||
3202 | for (int j = 0; j < 4; j++) | ||
3203 | { | ||
3204 | if (j == iCol) | ||
3205 | continue; | ||
3206 | Matrix4SetValue(ref minor, m,n, matrix[i, j]); | ||
3207 | n++; | ||
3208 | } | ||
3209 | m++; | ||
3210 | } | ||
3211 | |||
3212 | return minor; | ||
3213 | } | ||
3214 | |||
3215 | public static Matrix4 Transpose(Matrix4 pMat) | ||
3216 | { | ||
3217 | Matrix4 transposeMatrix = new Matrix4(); | ||
3218 | for (int i = 0; i < 4; i++) | ||
3219 | for (int j = 0; j < 4; j++) | ||
3220 | Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); | ||
3221 | return transposeMatrix; | ||
3222 | } | ||
3223 | |||
3224 | public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) | ||
3225 | { | ||
3226 | switch (r) | ||
3227 | { | ||
3228 | case 0: | ||
3229 | switch (c) | ||
3230 | { | ||
3231 | case 0: | ||
3232 | pMat.M11 = val; | ||
3233 | break; | ||
3234 | case 1: | ||
3235 | pMat.M12 = val; | ||
3236 | break; | ||
3237 | case 2: | ||
3238 | pMat.M13 = val; | ||
3239 | break; | ||
3240 | case 3: | ||
3241 | pMat.M14 = val; | ||
3242 | break; | ||
3243 | } | ||
3244 | |||
3245 | break; | ||
3246 | case 1: | ||
3247 | switch (c) | ||
3248 | { | ||
3249 | case 0: | ||
3250 | pMat.M21 = val; | ||
3251 | break; | ||
3252 | case 1: | ||
3253 | pMat.M22 = val; | ||
3254 | break; | ||
3255 | case 2: | ||
3256 | pMat.M23 = val; | ||
3257 | break; | ||
3258 | case 3: | ||
3259 | pMat.M24 = val; | ||
3260 | break; | ||
3261 | } | ||
3262 | |||
3263 | break; | ||
3264 | case 2: | ||
3265 | switch (c) | ||
3266 | { | ||
3267 | case 0: | ||
3268 | pMat.M31 = val; | ||
3269 | break; | ||
3270 | case 1: | ||
3271 | pMat.M32 = val; | ||
3272 | break; | ||
3273 | case 2: | ||
3274 | pMat.M33 = val; | ||
3275 | break; | ||
3276 | case 3: | ||
3277 | pMat.M34 = val; | ||
3278 | break; | ||
3279 | } | ||
3280 | |||
3281 | break; | ||
3282 | case 3: | ||
3283 | switch (c) | ||
3284 | { | ||
3285 | case 0: | ||
3286 | pMat.M41 = val; | ||
3287 | break; | ||
3288 | case 1: | ||
3289 | pMat.M42 = val; | ||
3290 | break; | ||
3291 | case 2: | ||
3292 | pMat.M43 = val; | ||
3293 | break; | ||
3294 | case 3: | ||
3295 | pMat.M44 = val; | ||
3296 | break; | ||
3297 | } | ||
3298 | |||
3299 | break; | ||
3300 | } | ||
3301 | } | ||
3302 | |||
3303 | private static float determinant3x3(Matrix4 pMat) | ||
3304 | { | ||
3305 | float det = 0; | ||
3306 | float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; | ||
3307 | float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; | ||
3308 | float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; | ||
3309 | float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; | ||
3310 | float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; | ||
3311 | float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; | ||
3312 | |||
3313 | det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); | ||
3314 | return det; | ||
3315 | } | ||
3316 | |||
3317 | private static void DMassCopy(ref d.Mass src, ref d.Mass dst) | ||
3318 | { | ||
3319 | dst.c.W = src.c.W; | ||
3320 | dst.c.X = src.c.X; | ||
3321 | dst.c.Y = src.c.Y; | ||
3322 | dst.c.Z = src.c.Z; | ||
3323 | dst.mass = src.mass; | ||
3324 | dst.I.M00 = src.I.M00; | ||
3325 | dst.I.M01 = src.I.M01; | ||
3326 | dst.I.M02 = src.I.M02; | ||
3327 | dst.I.M10 = src.I.M10; | ||
3328 | dst.I.M11 = src.I.M11; | ||
3329 | dst.I.M12 = src.I.M12; | ||
3330 | dst.I.M20 = src.I.M20; | ||
3331 | dst.I.M21 = src.I.M21; | ||
3332 | dst.I.M22 = src.I.M22; | ||
3333 | } | ||
3334 | |||
3335 | public override void SetMaterial(int pMaterial) | 3159 | public override void SetMaterial(int pMaterial) |
3336 | { | 3160 | { |
3337 | m_material = pMaterial; | 3161 | m_material = pMaterial; |
@@ -3365,7 +3189,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
3365 | // m_assetFailed = false; | 3189 | // m_assetFailed = false; |
3366 | 3190 | ||
3367 | // m_log.DebugFormat( | 3191 | // m_log.DebugFormat( |
3368 | // "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", | 3192 | // "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", |
3369 | // _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name); | 3193 | // _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name); |
3370 | 3194 | ||
3371 | m_taintshape = true; | 3195 | m_taintshape = true; |
@@ -3377,6 +3201,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
3377 | "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}", | 3201 | "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}", |
3378 | _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName); | 3202 | _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName); |
3379 | } | 3203 | } |
3380 | } | 3204 | } |
3381 | } | 3205 | } |
3382 | } \ No newline at end of file | 3206 | } \ No newline at end of file |