internal void Dispose()
{
if (Body.isInWorld())
{
m_parent_scene.removeFromWorld(Body);
}
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
}
m_aMotor.Dispose(); m_aMotor = null;
ClosestCastResult.Dispose(); ClosestCastResult = null;
Body.Dispose(); Body = null;
Shell.Dispose(); Shell = null;
tempQuat1.Dispose();
tempTrans1.Dispose();
tempVector1.Dispose();
tempVector2.Dispose();
tempVector3.Dispose();
tempVector4.Dispose();
tempVector5RayCast.Dispose();
tempVector6RayCast.Dispose();
}
internal void Dispose()
{
m_parent_scene.RemoveAvatarFromList(this);
if (Body != null)
{
if (Body.isInWorld())
{
m_parent_scene.removeFromWorld(this, Body);
m_parent_scene.RemoveCollisionObject(Body);
}
Body.Dispose();
Body = null;
}
if (m_aMotor != null)
{
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
}
m_aMotor.Dispose(); m_aMotor = null;
}
if (ClosestCastResult != null)
{
ClosestCastResult.Dispose(); ClosestCastResult = null;
}
if (Shell != null)
{
Shell.Dispose(); Shell = null;
}
tempQuat1.Dispose();
tempTrans1.Dispose();
tempVector1.Dispose();
tempVector2.Dispose();
tempVector3.Dispose();
tempVector4.Dispose();
tempVector5RayCast.Dispose();
tempVector6RayCast.Dispose();
}
/// <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 void CheckIfStandingOnObject()
{
float capsuleHalfHeight = ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) * 0.5f);
tempVector5RayCast.setValue(m_position.X, m_position.Y, m_position.Z);
tempVector6RayCast.setValue(m_position.X, m_position.Y, m_position.Z - capsuleHalfHeight * 1.1f);
ClosestCastResult.Dispose();
ClosestCastResult = new ClosestNotMeRayResultCallback(Body);
try
{
m_parent_scene.getBulletWorld().rayTest(tempVector5RayCast, tempVector6RayCast, ClosestCastResult);
}
catch (AccessViolationException)
{
m_log.Debug("BAD!");
}
if (ClosestCastResult.hasHit())
{
if (tempVector7RayCast != null)
{
tempVector7RayCast.Dispose();
}
//tempVector7RayCast = ClosestCastResult.getHitPointWorld();
/*if (tempVector7RayCast == null) // null == no result also
* {
* CollidingObj = false;
* IsColliding = false;
* CollidingGround = false;
*
* return;
* }
* float zVal = tempVector7RayCast.getZ();
* if (zVal != 0)
* m_log.Debug("[PHYSICS]: HAAAA");
* if (zVal < m_position.Z && zVal > ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) *0.5f))
* {
* CollidingObj = true;
* IsColliding = true;
* }
* else
* {
* CollidingObj = false;
* IsColliding = false;
* CollidingGround = false;
* }*/
//height+2*radius = capsule full length
//CollidingObj = true;
//IsColliding = true;
m_iscolliding = true;
}
else
{
//CollidingObj = false;
//IsColliding = false;
//CollidingGround = false;
m_iscolliding = false;
}
}
public async void OnMouseMove(object sender, SceneMouseEventArgs e)
{
//var state = Mouse.GetState( );
//if ( !state.IsButtonDown( MouseButton.Left ) ) return;
var dynamicScene = sender as DynamicScene;
if (dynamicScene == null)
{
return;
}
var collisionObject = selectedCollisionObject;
var scenarioObject = collisionObject?.UserObject as ObjectBlock;
if (scenarioObject == null)
{
return;
}
var mouse = new
{
Close = e.Camera.UnProject(new Vector2(e.ScreenCoordinates.X, e.ScreenCoordinates.Y), -1),
Far = e.Camera.UnProject(new Vector2(e.ScreenCoordinates.X, e.ScreenCoordinates.Y), 1)
};
var rayCullCallback = new ClosestRayResultCullFaceCallback(mouse.Close, mouse.Far, false)
{
CollisionFilterGroup = CollisionFilterGroups.AllFilter,
CollisionFilterMask = CollisionFilterGroups.StaticFilter
};
dynamicScene.CollisionManager.World.RayTest(mouse.Close, mouse.Far, rayCullCallback);
var rayCallback = new ClosestNotMeRayResultCallback(collisionObject, mouse.Close, mouse.Far)
{
CollisionFilterGroup = CollisionFilterGroups.AllFilter,
CollisionFilterMask = CollisionFilterGroups.StaticFilter
};
// GLDebug.Clear();
dynamicScene.CollisionManager.World.RayTest(mouse.Close, mouse.Far, rayCallback);
if (!rayCallback.HasHit)
{
return;
}
var splice = dynamicScene.Camera.Position;
if (rayCullCallback.HasHit && rayCullCallback.ClosestHitFraction < rayCallback.ClosestHitFraction)
{
splice = rayCullCallback.HitPointWorld;
}
Ray positionRay = new Ray(mouse.Close, mouse.Far);
Vector3 center;
float radius;
collisionObject.CollisionShape.GetBoundingSphere(out center, out radius);
var origin = mouse.Close + (mouse.Far - mouse.Close) * (rayCallback.ClosestHitFraction - 0.01f);
var surfaceNormal = rayCallback.HitSurfaceNormal;
var surfaceTangent = Vector3.Cross(rayCallback.HitSurfaceTangent, rayCallback.HitNormalWorld);
var surfaceBitangent = rayCallback.HitSurfaceTangent;
var basisMatrix = new Matrix4(new Vector4(surfaceTangent), new Vector4(surfaceBitangent),
new Vector4(rayCallback.HitSurfaceNormal), new Vector4(0, 0, 0, 1));
GLDebug.QueuePointDraw(rayCallback.HitPointWorld);
var destination = rayCallback.HitPointWorld;
GLDebug.QueueLineDraw(destination, destination + surfaceNormal * 1.5f);
GLDebug.QueueLineDraw(destination, destination + surfaceTangent * 0.5f);
GLDebug.QueueLineDraw(destination, destination + surfaceBitangent * 0.5f);
var convexCallback = new ClosestNotMeConvexResultCallback(collisionObject,
collisionObject.WorldTransform.ExtractTranslation( ), destination)
{
CollisionFilterGroup = CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter,
CollisionFilterMask = CollisionFilterGroups.StaticFilter
};
var convexShape = collisionObject.CollisionShape.IsConvex
? ( ConvexShape )collisionObject.CollisionShape
: null;
var extractRotation = Matrix4.CreateFromQuaternion(collisionObject.WorldTransform.ExtractRotation( ));
var vector3 = destination + surfaceNormal * 1.5f;
var extractTranslation = Matrix4.CreateTranslation(vector3);
var from = basisMatrix * extractTranslation;
var to = basisMatrix * Matrix4.CreateTranslation(destination);
GLDebug.QueueLineDraw(from.ExtractTranslation( ), to.ExtractTranslation( ), Color.Red.ToColorF( ).RGBA.Xyz);
await Task.Run(
() => dynamicScene.CollisionManager.World.ConvexSweepTest(convexShape, from, to, convexCallback));
if (convexCallback.HasHit)
{
Vector3 linVel;
Vector3 angVel;
TransformUtil.CalculateVelocity(from, to, 1.0f, out linVel, out angVel);
Matrix4 T;
TransformUtil.IntegrateTransform(from, linVel, angVel, convexCallback.ClosestHitFraction, out T);
//.WorldTransform = basisMatrix * T;
//var instanceBasis =
// scenarioObject.InstanceBasisMatrices[selectedCollisionObject.UserIndex];
//scenarioObject.AssignInstanceBasisTransform(selectedCollisionObject.UserIndex, basisMatrix);
//var upAxis = basisMatrix.Row2.Xyz.Normalized();
//GLDebug.QueueLineDraw(0, selectedCollisionObject.WorldTransform.ExtractTranslation(),
// selectedCollisionObject.WorldTransform.ExtractTranslation() + upAxis);
//var instanceRotation = scenarioObject.InstanceRotations[selectedCollisionObject.UserIndex];
//////selectedScenarioObject.SetAxisAlignedRotation( selectedCollisionObject.UserIndex, rotation );
//var R = Quaternion.FromAxisAngle(upAxis, _axisAlignedRotation);
collisionObject.WorldTransform = T;
//debugPoint2 = T.ExtractTranslation();
//debugPoint3 = convexCallback.HitPointWorld + convexCallback.HitNormalWorld;
//// selectedCollisionObject.WorldTransform = T;
////var localMatrix = scenarioObject.collisionSpaceMatrix.Inverted() * t.Inverted() * T;
//// scenarioObject.InstanceRotations[selectedCollisionObject.UserIndex] = instanceRotation;
//selectedScenarioObject.SetAxisAlignedRotation(selectedCollisionObject.UserIndex, _axisAlignedRotation);
//scenarioObject.InstancePositions[selectedCollisionObject.UserIndex] = (scenarioObject.collisionSpaceMatrix.Inverted() * T).ExtractTranslation();
//localMatrix.ExtractTranslation();
}
}
