public static void Multiply(ref btTransform t, ref btVector3 x, out btVector3 result)
{
result.X = t.Basis.el0.dot(x) + t.Origin.X;
result.Y = t.Basis.el1.dot(x) + t.Origin.Y;
result.Z = t.Basis.el2.dot(x) + t.Origin.Z;
result.W = 0;
}
public override void batchedUnitVectorGetSupportingVertexWithoutMargin( btVector3[] vectors, btVector3[] supportVerticesOut, int numVectors )
{
for( int i = 0; i < numVectors; i++ )
{
localGetSupportingVertexWithoutMargin( ref vectors[i], out supportVerticesOut[i] );
}
}
// See also geometrictools.com
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
double SegmentSqrDistance( ref btVector3 from, ref btVector3 to, ref btVector3 p, out btVector3 nearest )
{
btVector3 diff = p - from;
btVector3 v = to - from;
double t = v.dot( diff );
if( t > 0 )
{
double dotVV = v.dot( v );
if( t < dotVV )
{
t /= dotVV;
diff.AddScale( ref v, -t, out diff );
//diff -= t * v;
}
else
{
t = 1;
diff.Sub( ref v, out diff );
//diff -= v;
}
}
else
t = 0;
from.AddScale( ref v, t, out nearest );
//nearest = from + t * v;
return diff.dot( ref diff );
}
public void addPoint( ref btVector3 point, bool recalculateLocalAabb = true )
{
m_unscaledPoints.Add( point );
if( recalculateLocalAabb )
recalcLocalAabb();
}
public void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
{
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
m_hasResult = true;
}
public virtual void project( ref btTransform trans, ref btVector3 dir
, ref double min, ref double max
, out btVector3 witnesPtMin, out btVector3 witnesPtMax )
{
btVector3 localAxis; trans.m_basis.ApplyInverse( ref dir, out localAxis );
btVector3 tmpv;
localGetSupportingVertex( ref localAxis, out tmpv );
btVector3 vtx1; trans.Apply( ref tmpv, out vtx1 );
localAxis.Invert( out localAxis );
localGetSupportingVertex( ref localAxis, out tmpv );
btVector3 vtx2; trans.Apply( ref tmpv, out vtx2 );
min = vtx1.dot( ref dir );
max = vtx2.dot( ref dir );
witnesPtMax = vtx2;
witnesPtMin = vtx1;
if( min > max )
{
double tmp = min;
min = max;
max = tmp;
witnesPtMax = vtx1;
witnesPtMin = vtx2;
}
}
public override void drawContactPoint( ref btVector3 PointOnB, ref btVector3 normalOnB, double distance, int lifeTime, ref btVector3 color )
{
btVector3 tmpD;
PointOnB.Add( ref normalOnB, out tmpD );
drawLine( ref PointOnB, ref tmpD, ref color, ref color );
}
static void convexHullSupport( ref btVector3 localDirOrg, btVector3[] points, int numPoints, ref btVector3 localScaling, out btVector3 result )
{
btVector3 vec; localDirOrg.Mult( ref localScaling, out vec );
double maxDot;
long ptIndex = vec.maxDot( points, numPoints, out maxDot );
Debug.Assert( ptIndex >= 0 );
points[ptIndex].Mult( ref localScaling, out result );
}
public void setSafeMargin( ref btVector3 halfExtents, double defaultMarginMultiplier = 0.1f )
{
//see http://code.google.com/p/bullet/issues/detail?id=349
//this margin check could could be added to other collision shapes too,
//or add some assert/warning somewhere
double minDimension = halfExtents[halfExtents.minAxis()];
setSafeMargin( minDimension, defaultMarginMultiplier );
}
public override void localGetSupportingVertexWithoutMargin( ref btVector3 vec, out btVector3 result )
{
btVector3 halfExtents; getHalfExtentsWithoutMargin( out halfExtents );
result = new btVector3( btScalar.btFsels( vec.x, halfExtents.x, -halfExtents.x ),
btScalar.btFsels( vec.y, halfExtents.y, -halfExtents.y ),
btScalar.btFsels( vec.z, halfExtents.z, -halfExtents.z ) );
}
btStaticPlaneShape( ref btVector3 planeOrigin, ref btVector3 planeNormal ) : base()
{
planeNormal.normalized( out m_planeNormal );
m_planeConstant = planeOrigin.dot( ref planeNormal );
m_localScaling = btVector3.Zero;
m_shapeType = BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE;
// Debug.Assert( btFuzzyZero(m_planeNormal.length() - btScalar.BT_ONE) );
}
public static void AabbExpand( ref btVector3 aabbMin,
ref btVector3 aabbMax,
ref btVector3 expansionMin,
ref btVector3 expansionMax )
{
aabbMin.Add( ref expansionMin, out aabbMin );
aabbMin.Add( ref expansionMax, out aabbMin );
}
public btTriangleRaycastCallback( ref btVector3 from, ref btVector3 to, EFlags flags = EFlags.kF_None )
{
m_from = ( from );
m_to = ( to );
//@BP Mod
m_flags = ( flags );
m_hitFraction = ( btScalar.BT_ONE );
}
public void setPoints( btVector3[] points, int numPoints, bool computeAabb, ref btVector3 localScaling )
{
m_unscaledPoints = points;
m_numPoints = numPoints;
m_localScaling = localScaling;
if( computeAabb )
recalcLocalAabb();
}
public static bool TestAabbAgainstAabb2(btVector3 aabbMin1, btVector3 aabbMax1,
btVector3 aabbMin2, btVector3 aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.X > aabbMax2.X || aabbMax1.X < aabbMin2.X) ? false : overlap;
overlap = (aabbMin1.Z > aabbMax2.Z || aabbMax1.Z < aabbMin2.Z) ? false : overlap;
overlap = (aabbMin1.Y > aabbMax2.Y || aabbMax1.Y < aabbMin2.Y) ? false : overlap;
return overlap;
}
/// conservative test for overlap between two aabbs
public static bool TestPointAgainstAabb2( ref btVector3 aabbMin1, ref btVector3 aabbMax1,
ref btVector3 point )
{
bool overlap = true;
overlap = ( aabbMin1.x > point.x || aabbMax1.x < point.x ) ? false : overlap;
overlap = ( aabbMin1.z > point.z || aabbMax1.z < point.z ) ? false : overlap;
overlap = ( aabbMin1.y > point.y || aabbMax1.y < point.y ) ? false : overlap;
return overlap;
}
/// conservative test for overlap between two aabbs
public static bool TestAabbAgainstAabb2( ref btVector3 aabbMin1, ref btVector3 aabbMax1,
ref btVector3 aabbMin2, ref btVector3 aabbMax2 )
{
bool overlap = true;
overlap = ( aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x ) ? false : overlap;
overlap = ( aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z ) ? false : overlap;
overlap = ( aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y ) ? false : overlap;
return overlap;
}
public virtual void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
{
if( depth < m_distance )
{
m_normalOnSurfaceB = normalOnBInWorld;
m_closestPointInB = pointInWorld;
m_distance = depth;
}
}
public static int btOutcode( ref btVector3 p, ref btVector3 halfExtent )
{
return ( p.x < -halfExtent.x ? 0x01 : 0x0 ) |
( p.x > halfExtent.x ? 0x08 : 0x0 ) |
( p.y < -halfExtent.y ? 0x02 : 0x0 ) |
( p.y > halfExtent.y ? 0x10 : 0x0 ) |
( p.z < -halfExtent.z ? 0x4 : 0x0 ) |
( p.z > halfExtent.z ? 0x20 : 0x0 );
}
public override void drawLine( ref btVector3 from, ref btVector3 to, ref btVector3 fromColor, ref btVector3 toColor )
{
GL.Begin( PrimitiveType.Lines );
GL.Color4( fromColor.ToFloat4() );
GL.Vertex3( from.ToFloat4() );
GL.Color4( toColor.ToFloat4() );
GL.Vertex3( to.ToFloat4() );
GL.End();
}
public btConvexPointCloudShape( btVector3[] points, int numPoints, ref btVector3 localScaling, bool computeAabb = true )
{
m_localScaling = localScaling;
m_shapeType = BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
m_unscaledPoints = points;
m_numPoints = numPoints;
if( computeAabb )
recalcLocalAabb();
}
public btVector3 invXform(btVector3 inVec)
{
btVector3 v = inVec - Origin;
//return (m_basis.transpose() * v);
btMatrix3x3 tp;
btVector3 result;
//tp = Basis.transpose();
Basis.transpose(out tp);
btMatrix3x3.Multiply(ref tp, ref v, out result);
return result;
}
public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 result )
{
btVector3 halfExtents;
btVector3 margin = new btVector3( getMargin() );
m_implicitShapeDimensions.Add( ref margin, out halfExtents );
//halfExtents += margin;
result = new btVector3( btScalar.btFsels( vec.x, halfExtents.x, -halfExtents.x ),
btScalar.btFsels( vec.y, halfExtents.y, -halfExtents.y ),
btScalar.btFsels( vec.z, halfExtents.z, -halfExtents.z ) );
}
public override void setMargin( double collisionMargin )
{
//correct the m_implicitShapeDimensions for the margin
btVector3 oldMargin = new btVector3( getMargin(), getMargin(), getMargin() );
btVector3 implicitShapeDimensionsWithMargin; oldMargin.Add( ref m_implicitShapeDimensions, out implicitShapeDimensionsWithMargin );
base.setMargin( collisionMargin );
btVector3 newMargin = new btVector3( getMargin(), getMargin(), getMargin() );
implicitShapeDimensionsWithMargin.Sub( ref newMargin, out m_implicitShapeDimensions );
}
///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
public btSimpleDynamicsWorld(btDispatcher dispatcher
, btBroadphaseInterface pairCache
,btConstraintSolver constraintSolver
, btCollisionConfiguration collisionConfiguration)
: base( dispatcher, pairCache, collisionConfiguration )
{
m_constraintSolver = ( constraintSolver );
m_ownsConstraintSolver = ( false );
m_gravity = new btVector3( 0, 0, -9.8 );
}
/*
void setWorldTransform( ref btTransform worldTransform )
{
m_worldTransform = worldTransform;
}
void getWorldTransform( out btTransform result )
{
result = m_worldTransform;
}
*/
/// <summary>
/// Reset all vectors
/// </summary>
public void Clear()
{
m_deltaLinearVelocity = btVector3.Zero;
m_deltaAngularVelocity = btVector3.Zero;
m_pushVelocity = btVector3.Zero;
m_turnVelocity = btVector3.Zero;
m_deltaLinearVelocity = btVector3.Zero;
m_deltaAngularVelocity = btVector3.Zero;
m_pushVelocity = btVector3.Zero;
m_turnVelocity = btVector3.Zero;
}
public virtual void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth )
{
if( depth < m_distance )
{
m_hasResult = true;
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
//negative means penetration
m_distance = depth;
}
}
/*@brief Set the rotation using axis angle notation
@param axis The axis around which to rotate
@param angle The magnitude of the rotation in Radians */
public void setRotation( ref btVector3 axis, double _angle )
{
double d = axis.length();
if( d != 0.0 )
{
double s = btScalar.btSin( _angle * 0.5 ) / d;
setValue( axis.x * s, axis.y * s, axis.z * s,
btScalar.btCos( _angle * 0.5 ) );
}
else
x = y = z = w = 0;
}
public override void batchedUnitVectorGetSupportingVertexWithoutMargin( btVector3[] vectors, btVector3[] supportVerticesOut, int numVectors )
{
btVector3 halfExtents; getHalfExtentsWithoutMargin( out halfExtents );
for( int i = 0; i < numVectors; i++ )
{
btVector3 vec = vectors[i];
supportVerticesOut[i].setValue( btScalar.btFsels( vec.x, halfExtents.x, -halfExtents.x ),
btScalar.btFsels( vec.y, halfExtents.y, -halfExtents.y ),
btScalar.btFsels( vec.z, halfExtents.z, -halfExtents.z ) );
}
}
public override void setLocalScaling( ref btVector3 scaling )
{
btVector3 oldMargin = new btVector3( getMargin(), getMargin(), getMargin() );
btVector3 implicitShapeDimensionsWithMargin;
m_implicitShapeDimensions.Add( ref oldMargin, out implicitShapeDimensionsWithMargin );
btVector3 unScaledImplicitShapeDimensionsWithMargin;
implicitShapeDimensionsWithMargin.Div( ref m_localScaling, out unScaledImplicitShapeDimensionsWithMargin );
base.setLocalScaling( ref scaling );
unScaledImplicitShapeDimensionsWithMargin.Mult( ref m_localScaling, out unScaledImplicitShapeDimensionsWithMargin );
unScaledImplicitShapeDimensionsWithMargin.Sub(ref oldMargin, out m_implicitShapeDimensions );
}
public virtual void project(btTransform trans, btVector3 dir, SWIGTYPE_p_float minProj, SWIGTYPE_p_float maxProj, btVector3 witnesPtMin, btVector3 witnesPtMax)
{
BulletPINVOKE.btConvexHullShape_project(swigCPtr, btTransform.getCPtr(trans), btVector3.getCPtr(dir), SWIGTYPE_p_float.getCPtr(minProj), SWIGTYPE_p_float.getCPtr(maxProj), btVector3.getCPtr(witnesPtMin), btVector3.getCPtr(witnesPtMax));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btVector3 getScaledPoint(int i)
{
btVector3 ret = new btVector3(BulletPINVOKE.btConvexHullShape_getScaledPoint(swigCPtr, i), true);
return(ret);
}
public override void SetTerrain(float[] heightMap)
{
if (m_terrainShape != null)
{
DeleteTerrain();
}
float hfmax = -9000;
float hfmin = 90000;
for (int i = 0; i < heightMap.Length; i++)
{
if (Single.IsNaN(heightMap[i]) || Single.IsInfinity(heightMap[i]))
{
heightMap[i] = 0f;
}
hfmin = (heightMap[i] < hfmin) ? heightMap[i] : hfmin;
hfmax = (heightMap[i] > hfmax) ? heightMap[i] : hfmax;
}
// store this for later reference.
// Note, we're storing it after we check it for anomolies above
_origheightmap = heightMap;
hfmin = 0;
hfmax = 256;
m_terrainShape = new btHeightfieldTerrainShape((int)Constants.RegionSize, (int)Constants.RegionSize, heightMap,
1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
float AabbCenterX = Constants.RegionSize / 2f;
float AabbCenterY = Constants.RegionSize / 2f;
float AabbCenterZ = 0;
float temphfmin, temphfmax;
temphfmin = hfmin;
temphfmax = hfmax;
if (temphfmin < 0)
{
temphfmax = 0 - temphfmin;
temphfmin = 0 - temphfmin;
}
else if (temphfmin > 0)
{
temphfmax = temphfmax + (0 - temphfmin);
//temphfmin = temphfmin + (0 - temphfmin);
}
AabbCenterZ = temphfmax / 2f;
if (m_terrainPosition == null)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
else
{
try
{
m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
}
catch (ObjectDisposedException)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
}
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
TerrainBody.setUserPointer((IntPtr)0);
m_world.addRigidBody(TerrainBody);
}
public override void PostInitialise(IConfigSource config)
{
//m_config = config;
if (config != null)
{
IConfig physicsconfig = config.Configs["BulletPhysicsSettings"];
if (physicsconfig != null)
{
gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
avDensity = physicsconfig.GetFloat("av_density", 80f);
avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
//contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 4);
geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
if (Environment.OSVersion.Platform == PlatformID.Unix)
{
avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 65f);
avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 25);
bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 2f);
}
else
{
avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 65f);
avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 25);
bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 2f);
}
forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
}
}
lock (BulletLock)
{
worldAabbMax = new btVector3(m_region.RegionSizeX + 10f, m_region.RegionSizeY + 10f, m_region.RegionSizeZ);
m_broadphase = new btAxisSweep3(worldAabbMin, worldAabbMax, 16000);
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_solver = new btSequentialImpulseConstraintSolver();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_world.setGravity(m_gravity);
EnableCollisionInterface();
}
}
public btUniversalConstraint(btRigidBody rbA, btRigidBody rbB, btVector3 anchor, btVector3 axis1, btVector3 axis2) : this(BulletPINVOKE.new_btUniversalConstraint(btRigidBody.getCPtr(rbA), btRigidBody.getCPtr(rbB), btVector3.getCPtr(anchor), btVector3.getCPtr(axis1), btVector3.getCPtr(axis2)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btVector3 getAncorInB()
{
btVector3 ret = new btVector3(BulletPINVOKE.btSliderConstraint_getAncorInB(swigCPtr), true);
return(ret);
}
public btHingeConstraint(btRigidBody rbA, btRigidBody rbB, btVector3 pivotInA, btVector3 pivotInB, btVector3 axisInA, btVector3 axisInB) : this(BulletPINVOKE.new_btHingeConstraint__SWIG_1(btRigidBody.getCPtr(rbA), btRigidBody.getCPtr(rbB), btVector3.getCPtr(pivotInA), btVector3.getCPtr(pivotInB), btVector3.getCPtr(axisInA), btVector3.getCPtr(axisInB)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public override void ConvexDecompResult(ConvexResult result)
{
TriangleMesh trimesh = new TriangleMesh();
demo.trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6.0f, 6.0f, 6.0f);
if (output == null)
{
return;
}
output.WriteLine(string.Format("## Hull Piece {0} with {1} vertices and {2} triangles.", mHullCount, result.mHullVertices.Length, result.mHullIndices.Length / 3));
output.WriteLine(string.Format("usemtl Material{0}", mBaseCount));
output.WriteLine(string.Format("o Object{0}", mBaseCount));
foreach (Vector3 p in result.mHullVertices)
{
output.WriteLine(string.Format(floatFormat, "v {0:F9} {1:F9} {2:F9}", p.X, p.Y, p.Z));
}
//calc centroid, to shift vertices around center of mass
demo.centroid = Vector3.Zero;
AlignedVector3Array vertices = new AlignedVector3Array();
if (true)
{
foreach (Vector3 vertex in result.mHullVertices)
{
demo.centroid += Vector3.Multiply(vertex, localScaling);
}
}
demo.centroid /= (float)result.mHullVertices.Length;
if (true)
{
foreach (Vector3 vertex in result.mHullVertices)
{
vertices.Add(Vector3.Multiply(vertex, localScaling) - demo.centroid);
}
}
if (true)
{
int[] src = result.mHullIndices;
for (int i = 0; i < src.Length; i += 3)
{
int index0 = src[i];
int index1 = src[i + 1];
int index2 = src[i + 2];
Vector3 vertex0 = Vector3.Multiply(result.mHullVertices[index0], localScaling) - demo.centroid;
Vector3 vertex1 = Vector3.Multiply(result.mHullVertices[index1], localScaling) - demo.centroid;
Vector3 vertex2 = Vector3.Multiply(result.mHullVertices[index2], localScaling) - demo.centroid;
trimesh.AddTriangle(vertex0, vertex1, vertex2);
index0 += mBaseCount;
index1 += mBaseCount;
index2 += mBaseCount;
output.WriteLine(string.Format("f {0} {1} {2}", index0 + 1, index1 + 1, index2 + 1));
}
}
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#if SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray <btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices, planeEquations);
btAlignedObjectArray <btVector3> shiftedPlaneEquations;
for (int p = 0; p < planeEquations.size(); p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray <btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations, shiftedVertices);
btConvexHullShape *convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()), shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
ConvexHullShape convexShape = new ConvexHullShape(vertices);
#endif
convexShape.Margin = 0.01f;
convexShapes.Add(convexShape);
convexCentroids.Add(demo.centroid);
demo.CollisionShapes.Add(convexShape);
mBaseCount += result.mHullVertices.Length; // advance the 'base index' counter.
}
/// <summary>
/// This creates the Avatar's physical Surrogate at the position supplied
/// </summary>
/// <param name="npositionX"></param>
/// <param name="npositionY"></param>
/// <param name="npositionZ"></param>
// WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
// to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
// place that is safe to call this routine AvatarGeomAndBodyCreation.
private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
{
if (CAPSULE_LENGTH <= 0)
{
m_log.Warn("[PHYSICS]: The capsule size you specified in aurora.ini is invalid! Setting it to the smallest possible size!");
CAPSULE_LENGTH = 0.01f;
}
if (CAPSULE_RADIUS <= 0)
{
m_log.Warn("[PHYSICS]: The capsule size you specified in aurora.ini is invalid! Setting it to the smallest possible size!");
CAPSULE_RADIUS = 0.01f;
}
Shell = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH);
if (m_bodyPosition == null)
{
m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ);
}
m_bodyPosition.setValue(npositionX, npositionY, npositionZ);
if (m_bodyOrientation == null)
{
m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90));
}
if (m_bodyTransform == null)
{
m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
}
else
{
m_bodyTransform.Dispose();
m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
}
if (m_bodyMotionState == null)
{
m_bodyMotionState = new btDefaultMotionState(m_bodyTransform);
}
else
{
m_bodyMotionState.setWorldTransform(m_bodyTransform);
}
m_mass = Mass;
Body = new btRigidBody(m_mass, m_bodyMotionState, Shell);
// this is used for self identification. User localID instead of body handle
Body.setUserPointer(new IntPtr((int)m_localID));
if (ClosestCastResult != null)
{
ClosestCastResult.Dispose();
}
ClosestCastResult = new ClosestNotMeRayResultCallback(Body);
m_parent_scene.AddRigidBody(Body);
Body.setActivationState(4);
if (m_aMotor != null)
{
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
m_aMotor.Dispose();
}
m_aMotor = null;
}
m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody,
m_parent_scene.TransZero,
m_parent_scene.TransZero, false);
m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero);
m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero);
}
public int get_limit_motor_info2(btRotationalLimitMotor limot, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB, btTypedConstraint.btConstraintInfo2 info, int row, btVector3 ax1, int rotational)
{
int ret = BulletPINVOKE.btGeneric6DofConstraint_get_limit_motor_info2__SWIG_1(swigCPtr, btRotationalLimitMotor.getCPtr(limot), btTransform.getCPtr(transA), btTransform.getCPtr(transB), btVector3.getCPtr(linVelA), btVector3.getCPtr(linVelB), btVector3.getCPtr(angVelA), btVector3.getCPtr(angVelB), btTypedConstraint.btConstraintInfo2.getCPtr(info), row, btVector3.getCPtr(ax1), rotational);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public btVector3 getAxis(int axis_index)
{
btVector3 ret = new btVector3(BulletPINVOKE.btGeneric6DofConstraint_getAxis(swigCPtr, axis_index), true);
return(ret);
}
public void getInfo2NonVirtual(btTypedConstraint.btConstraintInfo2 info, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB)
{
BulletPINVOKE.btGeneric6DofConstraint_getInfo2NonVirtual(swigCPtr, btTypedConstraint.btConstraintInfo2.getCPtr(info), btTransform.getCPtr(transA), btTransform.getCPtr(transB), btVector3.getCPtr(linVelA), btVector3.getCPtr(linVelB), btVector3.getCPtr(angVelA), btVector3.getCPtr(angVelB));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
protected virtual void IntegrateTransforms(float timeStep)
{
BulletGlobals.StartProfile("integrateTransforms");
IndexedMatrix predictedTrans;
int length = m_nonStaticRigidBodies.Count;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("IntegrateTransforms [{0}]", length);
}
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.SetHitFraction(1f);
if (body.IsActive() && (!body.IsStaticOrKinematicObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
float squareMotion = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
//if (body.GetCcdSquareMotionThreshold() != 0 && body.GetCcdSquareMotionThreshold() < squareMotion)
if (GetDispatchInfo().m_useContinuous&& body.GetCcdSquareMotionThreshold() != 0.0f && body.GetCcdSquareMotionThreshold() < squareMotion)
{
BulletGlobals.StartProfile("CCD motion clamping");
if (body.GetCollisionShape().IsConvex())
{
gNumClampedCcdMotions++;
using (ClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get())
{
sweepResults.Initialize(body, body.GetWorldTransform()._origin, predictedTrans._origin, GetBroadphase().GetOverlappingPairCache(), GetDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
SphereShape tmpSphere = BulletGlobals.SphereShapePool.Get();
tmpSphere.Initialize(body.GetCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
sweepResults.m_allowedPenetration = GetDispatchInfo().GetAllowedCcdPenetration();
sweepResults.m_collisionFilterGroup = body.GetBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.GetBroadphaseProxy().m_collisionFilterMask;
IndexedMatrix modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans._basis = body.GetWorldTransform()._basis;
modifiedPredictedTrans._origin = predictedTrans._origin;
ConvexSweepTest(tmpSphere, body.GetWorldTransform(), modifiedPredictedTrans, sweepResults, 0f);
if (sweepResults.HasHit() && (sweepResults.m_closestHitFraction < 1.0f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body.SetHitFraction(sweepResults.m_closestHitFraction);
body.PredictIntegratedTransform(timeStep * body.GetHitFraction(), out predictedTrans);
body.SetHitFraction(0.0f);
body.ProceedToTransform(ref predictedTrans);
#if false
btVector3 linVel = body.getLinearVelocity();
float maxSpeed = body.getCcdMotionThreshold() / getSolverInfo().m_timeStep;
float maxSpeedSqr = maxSpeed * maxSpeed;
if (linVel.LengthSquared() > maxSpeedSqr)
{
linVel.normalize();
linVel *= maxSpeed;
body.setLinearVelocity(linVel);
float ms2 = body.getLinearVelocity().LengthSquared();
body.predictIntegratedTransform(timeStep, predictedTrans);
float sm2 = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
float smt = body.getCcdSquareMotionThreshold();
printf("sm2=%f\n", sm2);
}
#else
//response between two dynamic objects without friction, assuming 0 penetration depth
float appliedImpulse = 0.0f;
float depth = 0.0f;
appliedImpulse = ContactConstraint.ResolveSingleCollision(body, sweepResults.m_hitCollisionObject, ref sweepResults.m_hitPointWorld, ref sweepResults.m_hitNormalWorld, GetSolverInfo(), depth);
#endif
continue;
}
BulletGlobals.SphereShapePool.Free(tmpSphere);
}
}
BulletGlobals.StopProfile();
}
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
public btVector3 GetPointForAngle(float fAngleInRadians, float fLength)
{
btVector3 ret = new btVector3(BulletPINVOKE.btConeTwistConstraint_GetPointForAngle(swigCPtr, fAngleInRadians, fLength), true);
return(ret);
}
public override void batchedUnitVectorGetSupportingVertexWithoutMargin(btVector3 vectors, btVector3 supportVerticesOut, int numVectors)
{
BulletPINVOKE.btConvexHullShape_batchedUnitVectorGetSupportingVertexWithoutMargin(swigCPtr, btVector3.getCPtr(vectors), btVector3.getCPtr(supportVerticesOut), numVectors);
}
public btMultiSphereShape(btVector3 positions, SWIGTYPE_p_float radi, int numSpheres) : this(BulletPINVOKE.new_btMultiSphereShape(btVector3.getCPtr(positions), SWIGTYPE_p_float.getCPtr(radi), numSpheres), true)
{
}
public void getInfo2InternalUsingFrameOffset(btTypedConstraint.btConstraintInfo2 info, btTransform transA, btTransform transB, btVector3 angVelA, btVector3 angVelB)
{
BulletPINVOKE.btHingeConstraint_getInfo2InternalUsingFrameOffset(swigCPtr, btTypedConstraint.btConstraintInfo2.getCPtr(info), btTransform.getCPtr(transA), btTransform.getCPtr(transB), btVector3.getCPtr(angVelA), btVector3.getCPtr(angVelB));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btVector3 unQuantize(SWIGTYPE_p_unsigned_short vecIn)
{
btVector3 ret = new btVector3(BulletPINVOKE.btQuantizedBvh_unQuantize(swigCPtr, SWIGTYPE_p_unsigned_short.getCPtr(vecIn)), true);
return(ret);
}
public void ProcessTaints(float timestep)
{
if (m_tainted_isPhysical != m_isPhysical)
{
if (m_tainted_isPhysical)
{
// Create avatar capsule and related ODE data
if (!(Shell == null && Body == null))
{
m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+ (Shell != null ? "Shell " : "")
+ (Body != null ? "Body " : ""));
}
AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z);
}
else
{
// destroy avatar capsule and related ODE data
Dispose();
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
}
m_isPhysical = m_tainted_isPhysical;
}
if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
{
if (Body != null)
{
m_pidControllerActive = true;
// no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
//d.JointDestroy(Amotor);
float prevCapsule = CAPSULE_LENGTH;
CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
Dispose();
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
AvatarGeomAndBodyCreation(m_position.X, m_position.Y,
m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
Velocity = Vector3.Zero;
}
else
{
m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
+ (Shell == null ? "Shell " : "")
+ (Body == null ? "Body " : ""));
}
}
if (m_taintRemove)
{
Dispose();
}
}
public virtual void rayTest(btVector3 rayFrom, btVector3 rayTo, btBroadphaseRayCallback rayCallback, btVector3 aabbMin)
{
BulletPINVOKE.btBroadphaseInterface_rayTest__SWIG_1(swigCPtr, btVector3.getCPtr(rayFrom), btVector3.getCPtr(rayTo), btBroadphaseRayCallback.getCPtr(rayCallback), btVector3.getCPtr(aabbMin));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(float timeStep)
{
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
if (Body == null)
{
return;
}
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
if (m_pidControllerActive == false)
{
m_zeroPosition.X = tempVector1.getX();
m_zeroPosition.Y = tempVector1.getY();
m_zeroPosition.Z = tempVector1.getZ();
}
//PidStatus = true;
Vector3 vec = Vector3.Zero;
Vector3 vel = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
vel *= 0.25f;
float movementdivisor = 1f;
if (!m_alwaysRun)
{
movementdivisor = walkDivisor;
}
else
{
movementdivisor = runDivisor;
}
// if velocity is zero, use position control; otherwise, velocity control
if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!m_zeroFlag)
{
m_zeroFlag = true;
m_zeroPosition.X = tempVector1.getX();
m_zeroPosition.Y = tempVector1.getY();
m_zeroPosition.Z = tempVector1.getZ();
}
if (m_pidControllerActive)
{
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2);
vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2);
if (m_flying)
{
vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P;
}
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
m_zeroFlag = false;
if (m_iscolliding && !m_flying)
{
// We're standing on something
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
}
else if (m_iscolliding && m_flying)
{
// We're flying and colliding with something
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
}
else if (!m_iscolliding && m_flying)
{
// we're in mid air suspended
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
// We don't want linear velocity to cause our avatar to bounce, so we check target Z and actual velocity X, Y
// rebound preventing
//if (m_target_velocity.Z < 0.025f && m_velocity.X < 0.25f && m_velocity.Y < 0.25f)
// m_zeroFlag = true;
}
if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f)
{
// We're colliding with something and we're not flying but we're moving
// This means we're walking or running.
Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P;
if (m_target_velocity.X > 0)
{
vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
}
if (m_target_velocity.Y > 0)
{
vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
else if (!m_iscolliding && !m_flying)
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
// d.Vector3 pos = d.BodyGetPosition(Body);
if (m_target_velocity.X > 0)
{
vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
}
if (m_target_velocity.Y > 0)
{
vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
if (m_flying)
{
vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) * 10;
}
}
if (m_flying)
{
// Slight PID correction
vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 1.5f);
//auto fly height. Kitto Flora
//d.Vector3 pos = d.BodyGetPosition(Body);
float target_altitude = m_parent_scene.GetTerrainHeightAtXY(m_position.X, m_position.Y) + m_parent_scene.minimumGroundFlightOffset;
if (m_position.Z < target_altitude)
{
vec.Z += (target_altitude - m_position.Z) * PID_P * 5.0f;
}
}
if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f))))
{
Body.setFriction(0.001f);
//m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString());
}
if (Body != null)
{
int activationstate = Body.getActivationState();
if (activationstate == 0)
{
Body.forceActivationState(1);
}
}
doImpulse(vec, true);
}
/// <summary>
/// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
/// </summary>
public void UpdatePositionAndVelocity()
{
if (Body == null)
{
return;
}
//int val = Environment.TickCount;
CheckIfStandingOnObject();
//m_log.DebugFormat("time:{0}", Environment.TickCount - val);
//IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
if (vec.X < -10.0f)
{
vec.X = 0.0f;
}
if (vec.Y < -10.0f)
{
vec.Y = 0.0f;
}
if (vec.X > m_parent_scene.m_region.RegionSizeX + 10.2f)
{
vec.X = m_parent_scene.m_region.RegionSizeX + 10.2f;
}
if (vec.Y > m_parent_scene.m_region.RegionSizeY + 10.2f)
{
vec.Y = m_parent_scene.m_region.RegionSizeY + 10.2f;
}
m_position.X = vec.X;
m_position.Y = vec.Y;
m_position.Z = vec.Z;
// Did we move last? = zeroflag
// This helps keep us from sliding all over
if (m_zeroFlag)
{
m_velocity.X = 0.0f;
m_velocity.Y = 0.0f;
m_velocity.Z = 0.0f;
// Did we send out the 'stopped' message?
if (!m_lastUpdateSent)
{
m_lastUpdateSent = true;
base.RequestPhysicsterseUpdate();
}
//Tell any listeners that we've stopped
base.TriggerMovementUpdate();
}
else
{
m_lastUpdateSent = false;
vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
m_velocity.X = (vec.X);
m_velocity.Y = (vec.Y);
m_velocity.Z = (vec.Z);
//m_log.Debug(m_target_velocity);
if (m_velocity.Z < -6 && !m_hackSentFall)
{
m_hackSentFall = true;
m_pidControllerActive = false;
}
else if (m_flying && !m_hackSentFly)
{
//m_hackSentFly = true;
//base.SendCollisionUpdate(new CollisionEventUpdate());
}
else
{
m_hackSentFly = false;
m_hackSentFall = false;
}
const float VELOCITY_TOLERANCE = 0.001f;
const float POSITION_TOLERANCE = 0.05f;
//Check to see whether we need to trigger the significant movement method in the presence
if (!RotationalVelocity.ApproxEquals(m_lastRotationalVelocity, VELOCITY_TOLERANCE) ||
!Velocity.ApproxEquals(m_lastVelocity, VELOCITY_TOLERANCE) ||
!Position.ApproxEquals(m_lastPosition, POSITION_TOLERANCE))
{
// Update the "last" values
m_lastPosition = Position;
m_lastRotationalVelocity = RotationalVelocity;
m_lastVelocity = Velocity;
base.RequestPhysicsterseUpdate();
base.TriggerSignificantMovement();
}
//Tell any listeners about the new info
base.TriggerMovementUpdate();
}
if (Body != null)
{
if (Body.getFriction() < 0.9f)
{
Body.setFriction(0.9f);
}
}
//if (Body != null)
// Body.clearForces();
}
public btVector3 getSpherePosition(int index)
{
btVector3 ret = new btVector3(BulletPINVOKE.btMultiSphereShape_getSpherePosition(swigCPtr, index), false);
return(ret);
}
public virtual void debugDrawObject(btTransform worldTransform, btCollisionShape shape, btVector3 color)
{
BulletPINVOKE.btCollisionWorld_debugDrawObject(swigCPtr, btTransform.getCPtr(worldTransform), btCollisionShape.getCPtr(shape), btVector3.getCPtr(color));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void reportBoxCastOverlappingNodex(btNodeOverlapCallback nodeCallback, btVector3 raySource, btVector3 rayTarget, btVector3 aabbMin, btVector3 aabbMax)
{
BulletPINVOKE.btQuantizedBvh_reportBoxCastOverlappingNodex(swigCPtr, btNodeOverlapCallback.getCPtr(nodeCallback), btVector3.getCPtr(raySource), btVector3.getCPtr(rayTarget), btVector3.getCPtr(aabbMin), btVector3.getCPtr(aabbMax));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public LocalConvexResult(btCollisionObject hitCollisionObject, btCollisionWorld.LocalShapeInfo localShapeInfo, btVector3 hitNormalLocal, btVector3 hitPointLocal, float hitFraction) : this(BulletPINVOKE.new_btCollisionWorld_LocalConvexResult(btCollisionObject.getCPtr(hitCollisionObject), btCollisionWorld.LocalShapeInfo.getCPtr(localShapeInfo), btVector3.getCPtr(hitNormalLocal), btVector3.getCPtr(hitPointLocal), hitFraction), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public override void SetTerrain(short[] shortheightMap)
{
if (m_terrainShape != null)
{
DeleteTerrain();
}
float hfmax = 256;
float hfmin = 0;
// store this for later reference.
// Note, we're storing it after we check it for anomolies above
_origheightmap = shortheightMap;
hfmin = 0;
hfmax = 256;
float[] heightmap = new float[m_region.RegionSizeX * m_region.RegionSizeX];
for (int i = 0; i < shortheightMap.Length; i++)
{
heightmap[i] = shortheightMap[i] / Constants.TerrainCompression;
}
m_terrainShape = new btHeightfieldTerrainShape(m_region.RegionSizeX, m_region.RegionSizeY, heightmap,
1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
float AabbCenterX = m_region.RegionSizeX / 2f;
float AabbCenterY = m_region.RegionSizeY / 2f;
float AabbCenterZ = 0;
float temphfmin, temphfmax;
temphfmin = hfmin;
temphfmax = hfmax;
if (temphfmin < 0)
{
temphfmax = 0 - temphfmin;
temphfmin = 0 - temphfmin;
}
else if (temphfmin > 0)
{
temphfmax = temphfmax + (0 - temphfmin);
//temphfmin = temphfmin + (0 - temphfmin);
}
AabbCenterZ = temphfmax / 2f;
if (m_terrainPosition == null)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
else
{
try
{
m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
}
catch (ObjectDisposedException)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
}
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
TerrainBody.setUserPointer((IntPtr)0);
m_world.addRigidBody(TerrainBody);
}
/// <summary>
/// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
/// </summary>
public void UpdatePositionAndVelocity()
{
if (Body == null)
{
return;
}
//int val = Environment.TickCount;
CheckIfStandingOnObject();
//m_log.DebugFormat("time:{0}", Environment.TickCount - val);
//IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
PhysicsVector vec = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
if (vec.X < -10.0f)
{
vec.X = 0.0f;
}
if (vec.Y < -10.0f)
{
vec.Y = 0.0f;
}
if (vec.X > (int)Constants.RegionSize + 10.2f)
{
vec.X = (int)Constants.RegionSize + 10.2f;
}
if (vec.Y > (int)Constants.RegionSize + 10.2f)
{
vec.Y = (int)Constants.RegionSize + 10.2f;
}
m_position.X = vec.X;
m_position.Y = vec.Y;
m_position.Z = vec.Z;
// Did we move last? = zeroflag
// This helps keep us from sliding all over
if (m_zeroFlag)
{
m_velocity.X = 0.0f;
m_velocity.Y = 0.0f;
m_velocity.Z = 0.0f;
// Did we send out the 'stopped' message?
if (!m_lastUpdateSent)
{
m_lastUpdateSent = true;
//base.RequestPhysicsterseUpdate();
}
}
else
{
m_lastUpdateSent = false;
vec = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
m_velocity.X = (vec.X);
m_velocity.Y = (vec.Y);
m_velocity.Z = (vec.Z);
//m_log.Debug(m_target_velocity);
if (m_velocity.Z < -6 && !m_hackSentFall)
{
m_hackSentFall = true;
m_pidControllerActive = false;
}
else if (m_flying && !m_hackSentFly)
{
//m_hackSentFly = true;
//base.SendCollisionUpdate(new CollisionEventUpdate());
}
else
{
m_hackSentFly = false;
m_hackSentFall = false;
}
}
if (Body != null)
{
if (Body.getFriction() < 0.9f)
{
Body.setFriction(0.9f);
}
}
//if (Body != null)
// Body.clearForces();
}
public btVector3 getAxis2()
{
btVector3 ret = new btVector3(BulletPINVOKE.btUniversalConstraint_getAxis2(swigCPtr), false);
return(ret);
}
public void getInfo2NonVirtual(btTypedConstraint.btConstraintInfo2 info, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, float rbAinvMass, float rbBinvMass)
{
BulletPINVOKE.btSliderConstraint_getInfo2NonVirtual(swigCPtr, btTypedConstraint.btConstraintInfo2.getCPtr(info), btTransform.getCPtr(transA), btTransform.getCPtr(transB), btVector3.getCPtr(linVelA), btVector3.getCPtr(linVelB), rbAinvMass, rbBinvMass);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}