public btQuaternion farthest(btQuaternion q)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
return FromIntPtr(BulletAPI_BtQuaternion_farthest(m_handle, q.Handle));
}
public float dot(btQuaternion q)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
return BulletAPI_BtQuaternion_dot(m_handle, q.Handle);
}
public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
{
m_position = pos;
m_zeroPosition = pos;
m_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_density = density;
heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
for (int i = 0; i < 11; i++)
{
m_colliderarr[i] = false;
}
for (int i = 0; i < 11; i++)
{
m_colliderGroundarr[i] = false;
}
CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
m_parent_scene.AddPhysicsActorTaint(this);
// m_name = avName;
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);
}
public override void Dispose()
{
lock (BulletLock)
{
disposeAllBodies();
m_world.Dispose();
m_broadphase.Dispose();
((btDefaultCollisionConfiguration)m_collisionConfiguration).Dispose();
((btSequentialImpulseConstraintSolver)m_solver).Dispose();
worldAabbMax.Dispose();
worldAabbMin.Dispose();
VectorZero.Dispose();
QuatIdentity.Dispose();
m_gravity.Dispose();
VectorZero = null;
QuatIdentity = null;
}
}
public BulletDotNETScene(string sceneIdentifier)
{
BulletLock = new object();
// m_sceneIdentifier = sceneIdentifier;
VectorZero = new btVector3(0, 0, 0);
QuatIdentity = new btQuaternion(0, 0, 0, 1);
TransZero = new btTransform(QuatIdentity, VectorZero);
m_gravity = new btVector3(0, 0, gravityz);
_origheightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
}
public BulletDotNETScene(string sceneIdentifier)
{
BulletLock = new object();
// m_sceneIdentifier = sceneIdentifier;
VectorZero = new btVector3(0, 0, 0);
QuatIdentity = new btQuaternion(0, 0, 0, 1);
TransZero = new btTransform(QuatIdentity, VectorZero);
m_gravity = new btVector3(0, 0, gravityz);
}
private void rotate(float timestep)
{
m_log.Debug("[PHYSICS]: _________ChangeRotate");
tempTransform2 = Body.getWorldTransform();
tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
tempTransform2.setRotation(tempOrientation2);
Body.setWorldTransform(tempTransform2);
resetCollisionAccounting();
m_taintrot = _orientation;
}
public btQuaternion slerp(btQuaternion q, float t)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
return FromIntPtr(BulletAPI_BtQuaternion_slerp(m_handle, q.Handle, t));
}
public btMatrix3x3(btQuaternion q)
{
m_handle = BulletAPI_CreateBtMatrix3x3Quaternion(q.Handle);
}
public void SetBody(float mass)
{
if (!IsPhysical || childrenPrim.Count == 0)
{
if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
tempMotionState1.Dispose();
if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
tempTransform2.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
tempPosition2.Dispose();
tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
tempInertia1.Dispose();
tempInertia1 = new btVector3(0, 0, 0);
prim_geom.calculateLocalInertia(mass, tempInertia1);
if (mass != 0)
_parent_scene.addActivePrim(this);
else
_parent_scene.remActivePrim(this);
// Body = new btRigidBody(mass, tempMotionState1, prim_geom);
//else
Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
if (prim_geom is btGImpactMeshShape)
{
((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
((btGImpactMeshShape)prim_geom).updateBound();
}
//Body.setCollisionFlags(Body.getCollisionFlags() | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//Body.setUserPointer((IntPtr) (int)m_localID);
_parent_scene.AddPrimToScene(this);
}
else
{
// bool hasTrimesh = false;
lock (childrenPrim)
{
foreach (BulletDotNETPrim chld in childrenPrim)
{
if (chld == null)
continue;
// if (chld.NeedsMeshing())
// hasTrimesh = true;
}
}
//if (hasTrimesh)
//{
ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
// createmesh returns null when it doesn't mesh.
/*
if (_mesh is Mesh)
{
}
else
{
m_log.Warn("[PHYSICS]: Can't link a OpenSim.Region.Physics.Meshing.Mesh object");
return;
}
*/
foreach (BulletDotNETPrim chld in childrenPrim)
{
if (chld == null)
continue;
Vector3 offset = chld.Position - Position;
Vector3 pos = new Vector3(offset.X, offset.Y, offset.Z);
pos *= Quaternion.Inverse(Orientation);
//pos *= Orientation;
offset = pos;
chld.ProcessGeomCreationAsTriMesh(offset, chld.Orientation);
_mesh.Append(chld._mesh);
}
setMesh(_parent_scene, _mesh);
//}
if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
tempMotionState1.Dispose();
if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
tempTransform2.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
tempPosition2.Dispose();
tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
tempInertia1.Dispose();
tempInertia1 = new btVector3(0, 0, 0);
prim_geom.calculateLocalInertia(mass, tempInertia1);
if (mass != 0)
_parent_scene.addActivePrim(this);
else
_parent_scene.remActivePrim(this);
// Body = new btRigidBody(mass, tempMotionState1, prim_geom);
//else
Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
if (prim_geom is btGImpactMeshShape)
{
((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
((btGImpactMeshShape)prim_geom).updateBound();
}
_parent_scene.AddPrimToScene(this);
}
if (IsPhysical)
changeAngularLock(0);
}
public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical)
{
tempPosition1 = new btVector3(0, 0, 0);
tempPosition2 = new btVector3(0, 0, 0);
tempPosition3 = new btVector3(0, 0, 0);
tempSize1 = new btVector3(0, 0, 0);
tempSize2 = new btVector3(0, 0, 0);
tempLinearVelocity1 = new btVector3(0, 0, 0);
tempLinearVelocity2 = new btVector3(0, 0, 0);
tempAngularVelocity1 = new btVector3(0, 0, 0);
tempAngularVelocity2 = new btVector3(0, 0, 0);
tempInertia1 = new btVector3(0, 0, 0);
tempInertia2 = new btVector3(0, 0, 0);
tempOrientation1 = new btQuaternion(0, 0, 0, 1);
tempOrientation2 = new btQuaternion(0, 0, 0, 1);
_parent_scene = parent_scene;
tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero);
tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero);
tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero);
tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero);
AxisLockLinearLow = new btVector3(-1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize);
int regionsize = (int)Constants.RegionSize;
if (regionsize == 256)
regionsize = 512;
AxisLockLinearHigh = new btVector3((int)Constants.RegionSize, (int)Constants.RegionSize, (int)Constants.RegionSize);
_target_velocity = Vector3.Zero;
_velocity = Vector3.Zero;
_position = pos;
m_taintposition = pos;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
// body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = null;
Body = null;
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
m_taintsize = _size;
_acceleration = Vector3.Zero;
m_rotationalVelocity = Vector3.Zero;
_orientation = rotation;
m_taintrot = _orientation;
_mesh = mesh;
_pbs = pbs;
_parent_scene = parent_scene;
if (pos.Z < 0)
m_isphysical = false;
else
{
m_isphysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
}
m_primName = primName;
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
}
private void changeshape(float timestep)
{
if (Body != null)
{
if (Body.Handle != IntPtr.Zero)
{
DisableAxisMotor();
_parent_scene.removeFromWorld(this, Body);
//Body.Dispose();
}
//Body = null;
// TODO: dispose parts that make up body
}
// Cleanup of old prim geometry and Bodies
if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
//disableBody();
}
}
try
{
//SetCollisionShape(null);
}
catch (System.AccessViolationException)
{
//prim_geom = IntPtr.Zero;
m_log.Error("[PHYSICS]: PrimGeom dead");
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
// Construction of new prim
ProcessGeomCreation();
tempPosition1.setValue(_position.X, _position.Y, _position.Z);
if (tempOrientation1.Handle != IntPtr.Zero)
tempOrientation1.Dispose();
tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W);
if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
tempTransform1.Dispose();
tempTransform1 = new btTransform(tempOrientation1, tempPosition1);
//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
if (IsPhysical)
{
SetBody(Mass);
// Re creates body on size.
// EnableBody also does setMass()
}
else
{
SetBody(0);
}
changeSelectedStatus(timestep);
if (childPrim)
{
if (_parent is BulletDotNETPrim)
{
BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
m_taintshape = false;
}
private void LimitRotation(float timestep)
{
btQuaternion rot = m_body.getWorldTransform().getRotation();
bool changed = false;
if (m_RollreferenceFrame != Quaternion.Identity)
{
if (rot.getX() >= m_RollreferenceFrame.X)
rot = new btQuaternion(rot.getX() - (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW());
if (rot.getX() <= -m_RollreferenceFrame.X)
rot = new btQuaternion(rot.getX() + (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW());
if (rot.getY() >= m_RollreferenceFrame.Y)
rot = new btQuaternion(rot.getX(), rot.getY() - (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW());
if (rot.getY() <= -m_RollreferenceFrame.Y)
rot = new btQuaternion(rot.getX(), rot.getY() + (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW());
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
rot = new btQuaternion(0,0,rot.getZ(),rot.getW());
changed = true;
}
if (changed)
{
btTransform trans = m_body.getWorldTransform();
trans.setRotation(rot);
m_body.setWorldTransform(trans);
}
}
public btTransform(btQuaternion q, btVector3 v)
{
m_handle = BulletAPI_CreateBtTransformQuaternionVector3(q.Handle, v.Handle);
}
/// <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 opensim.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 opensim.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 void UpdatePositionAndVelocity()
{
if (!m_isSelected)
{
if (_parent == null)
{
Vector3 pv = Vector3.Zero;
bool lastZeroFlag = _zeroFlag;
if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
tempPosition3.Dispose();
if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
tempTransform3.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
tempAngularVelocity1.Dispose();
if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
tempLinearVelocity1.Dispose();
tempTransform3 = Body.getInterpolationWorldTransform();
tempPosition3 = tempTransform3.getOrigin(); // vec
tempOrientation2 = tempTransform3.getRotation(); // ori
tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel
_torque = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
tempAngularVelocity1.getZ());
Vector3 l_position = Vector3.Zero;
Quaternion l_orientation = Quaternion.Identity;
m_lastposition = _position;
m_lastorientation = _orientation;
l_position.X = tempPosition3.getX();
l_position.Y = tempPosition3.getY();
l_position.Z = tempPosition3.getZ();
l_orientation.X = tempOrientation2.getX();
l_orientation.Y = tempOrientation2.getY();
l_orientation.Z = tempOrientation2.getZ();
l_orientation.W = tempOrientation2.getW();
if (l_position.X > ((int)Constants.RegionSize - 0.05f) || l_position.X < 0f || l_position.Y > ((int)Constants.RegionSize - 0.05f) || l_position.Y < 0f)
{
//base.RaiseOutOfBounds(l_position);
if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
{
_position = l_position;
//_parent_scene.remActivePrim(this);
if (_parent == null)
base.RequestPhysicsterseUpdate();
return;
}
else
{
if (_parent == null)
base.RaiseOutOfBounds(l_position);
return;
}
}
if (l_position.Z < -200000f)
{
// This is so prim that get lost underground don't fall forever and suck up
//
// Sim resources and memory.
// Disables the prim's movement physics....
// It's a hack and will generate a console message if it fails.
//IsPhysical = false;
//if (_parent == null)
//base.RaiseOutOfBounds(_position);
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_throttleUpdates = false;
// throttleCounter = 0;
_zeroFlag = true;
//outofBounds = true;
}
if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
&& (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
&& (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
{
_zeroFlag = true;
m_throttleUpdates = false;
}
else
{
//m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
_zeroFlag = false;
}
if (_zeroFlag)
{
_velocity.X = 0.0f;
_velocity.Y = 0.0f;
_velocity.Z = 0.0f;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
//_orientation.w = 0f;
//_orientation.X = 0f;
//_orientation.Y = 0f;
//_orientation.Z = 0f;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (!m_lastUpdateSent)
{
m_throttleUpdates = false;
// throttleCounter = 0;
m_rotationalVelocity = pv;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_lastUpdateSent = true;
}
}
else
{
if (lastZeroFlag != _zeroFlag)
{
if (_parent == null)
base.RequestPhysicsterseUpdate();
}
m_lastVelocity = _velocity;
_position = l_position;
_velocity.X = tempLinearVelocity1.getX();
_velocity.Y = tempLinearVelocity1.getY();
_velocity.Z = tempLinearVelocity1.getZ();
_acceleration = ((_velocity - m_lastVelocity) / 0.1f);
_acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
_velocity.Y - m_lastVelocity.Y / 0.1f,
_velocity.Z - m_lastVelocity.Z / 0.1f);
//m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
if (_velocity.ApproxEquals(pv, 0.5f))
{
m_rotationalVelocity = pv;
}
else
{
m_rotationalVelocity = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
}
//m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
_orientation.X = l_orientation.X;
_orientation.Y = l_orientation.Y;
_orientation.Z = l_orientation.Z;
_orientation.W = l_orientation.W;
m_lastUpdateSent = false;
//if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
//{
if (_parent == null)
base.RequestPhysicsterseUpdate();
// }
// else
// {
// throttleCounter++;
//}
}
m_lastposition = l_position;
if (forceenable)
{
Body.forceActivationState(1);
forceenable = false;
}
}
else
{
// Not a body.. so Make sure the client isn't interpolating
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
_zeroFlag = true;
}
}
}
public 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();
}
}
internal void Dispose()
{
//TODO:
DisableAxisMotor();
DisposeOfBody();
SetCollisionShape(null);
if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero)
{
tempMotionState3.Dispose();
tempMotionState3 = null;
}
if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero)
{
tempMotionState2.Dispose();
tempMotionState2 = null;
}
if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
{
tempMotionState1.Dispose();
tempMotionState1 = null;
}
if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero)
{
tempTransform4.Dispose();
tempTransform4 = null;
}
if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
{
tempTransform3.Dispose();
tempTransform3 = null;
}
if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
{
tempTransform2.Dispose();
tempTransform2 = null;
}
if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
{
tempTransform1.Dispose();
tempTransform1 = null;
}
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
{
tempOrientation2.Dispose();
tempOrientation2 = null;
}
if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero)
{
tempOrientation1.Dispose();
tempOrientation1 = null;
}
if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
{
tempInertia1.Dispose();
tempInertia1 = null;
}
if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero)
{
tempInertia2.Dispose();
tempInertia1 = null;
}
if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero)
{
tempAngularVelocity2.Dispose();
tempAngularVelocity2 = null;
}
if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
{
tempAngularVelocity1.Dispose();
tempAngularVelocity1 = null;
}
if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero)
{
tempLinearVelocity2.Dispose();
tempLinearVelocity2 = null;
}
if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
{
tempLinearVelocity1.Dispose();
tempLinearVelocity1 = null;
}
if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero)
{
tempSize2.Dispose();
tempSize2 = null;
}
if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero)
{
tempSize1.Dispose();
tempSize1 = null;
}
if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
{
tempPosition3.Dispose();
tempPosition3 = null;
}
if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
{
tempPosition2.Dispose();
tempPosition2 = null;
}
if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero)
{
tempPosition1.Dispose();
tempPosition1 = null;
}
if (AxisLockLinearLow != null && AxisLockLinearLow.Handle != IntPtr.Zero)
{
AxisLockLinearLow.Dispose();
AxisLockLinearLow = null;
}
if (AxisLockLinearHigh != null && AxisLockLinearHigh.Handle != IntPtr.Zero)
{
AxisLockLinearHigh.Dispose();
AxisLockLinearHigh = null;
}
}
public void setRotation(btQuaternion q)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
BulletAPI_BtMatrix3x3_setRotation(m_handle, q.Handle);
}
static void Main(string[] args)
{
btVector3 testvec = new btVector3(-2, 1, 0);
Console.WriteLine(String.Format("Original: {0}", testvec.testStr()));
btVector3 testvec2 = testvec.absolute();
Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr()));
Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2)));
Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis()));
btVector3 testvec3 = testvec.cross(testvec2);
Console.WriteLine(String.Format("cross: {0}", testvec3.testStr()));
Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2)));
Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2)));
Console.WriteLine(String.Format("dot: {0}", testvec.dot(testvec2)));
Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis()));
btVector3 testvec4 = testvec.normalized();
Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr()));
testvec4.setInterpolate3(testvec, testvec2, 0.5f);
Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr()));
testvec4.setValue(7f, -0.09f, 2.5f);
Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr()));
testvec4.setX(5.0f);
testvec4.setY(-0.25f);
testvec4.setZ(90f);
testvec.setValue(0, 0, -1024);
testvec2.setValue(256, 256, 1024);
Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr()));
btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50);
btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration();
btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig);
btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver();
btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol,
colconfig);
dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
btQuaternion testquat = new btQuaternion(50, 0, 0, 1);
btQuaternion testquatnorm = testquat.normalized();
Console.WriteLine(String.Format("testquat: {0}", testquat.testStr()));
Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr()));
Console.WriteLine(String.Format("testquatLength: {0}", testquat.length()));
Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length()));
float[] heightdata = new float[256*256];
for (int j=0;j<256*256;j++)
{
if (j%2==0)
heightdata[j] = 21f;
else
heightdata[j] = 28f;
}
btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256,
(int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.
PHY_FLOAT, false);
btCapsuleShape cap = new btCapsuleShape(0.23f, 3);
btTriangleMesh testMesh = new btTriangleMesh(true, false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false);
btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh);
meshtest.updateBound();
btRigidBody groundbody = new btRigidBody(0,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 256f / 2f))), obj,
new btVector3(0, 0, 0));
btRigidBody capbody = new btRigidBody(200,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 25))), cap,
new btVector3(0, 0, 0));
btRigidBody meshbody = new btRigidBody(200,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 29))), meshtest,
new btVector3(0, 0, 0));
btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo();
constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f,0.5f,0.5f));
constructioninfotest.m_localInertia = new btVector3(0, 0, 0);
constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)),
new btTransform(new btQuaternion(0,0,0,1),new btVector3(0,0.25f,0)));
constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0,0,0,1),new btVector3(0,0,0));
constructioninfotest.m_mass = 2000000;
constructioninfotest.m_linearDamping = 0;
constructioninfotest.m_angularDamping = 0;
constructioninfotest.m_friction = 0.1f;
constructioninfotest.m_restitution = 0;
constructioninfotest.m_linearSleepingThreshold = 0.8f;
constructioninfotest.m_angularSleepingThreshold = 1;
constructioninfotest.m_additionalDamping = false;
constructioninfotest.m_additionalDampingFactor = 0.005f;
constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f;
constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f;
constructioninfotest.m_additionalAngularDampingFactor = 0.01f;
constructioninfotest.commit();
btGImpactCollisionAlgorithm.registerAlgorithm(coldisp);
btRigidBody cubetest = new btRigidBody(constructioninfotest);
dynamicsWorld.addRigidBody(groundbody);
dynamicsWorld.addRigidBody(cubetest);
dynamicsWorld.addRigidBody(capbody);
dynamicsWorld.addRigidBody(meshbody);
int frame = 0;
for (int i = 0; i < 26; i++ )
{
int frames = dynamicsWorld.stepSimulation(((i%60) / 60f), 10, (1f / 60));
frame += frames;
Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr()));
Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr()));
Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr()));
}
dynamicsWorld.removeRigidBody(meshbody);
dynamicsWorld.removeRigidBody(capbody);
dynamicsWorld.removeRigidBody(cubetest);
dynamicsWorld.removeRigidBody(groundbody);
cubetest.Dispose();
groundbody.Dispose();
capbody.Dispose();
cap.Dispose();
obj.Dispose();
testbtAxisSweep3.Dispose();
dynamicsWorld.Dispose();
coldisp.Dispose();
colconfig.Dispose();
seqimpconssol.Dispose();
testvec.Dispose();
testvec2.Dispose();
testvec3.Dispose();
testvec4.Dispose();
}