public void Move(CollisionObject item, Matrix4 transform)
{
var worldTransform = item.WorldTransform;
var from = worldTransform.ExtractTranslation( );
var to = transform.ExtractTranslation( );
var closestConvexResultCallback = new ClosestNotMeConvexResultCallback(item, from, to)
{
CollisionFilterGroup = ( CollisionFilterGroups )(CollisionGroup.Objects | CollisionGroup.Level)
};
World.ConvexSweepTest(( ConvexShape )item.CollisionShape, worldTransform, transform, closestConvexResultCallback);
if (closestConvexResultCallback.HasHit)
{
Vector3 linVel;
Vector3 angVel;
TransformUtil.CalculateVelocity(worldTransform, transform, 1.0f, out linVel, out angVel);
Matrix4 T;
TransformUtil.IntegrateTransform(worldTransform, linVel, angVel,
closestConvexResultCallback.ClosestHitFraction, out T);
item.WorldTransform = T;
}
else
{
item.WorldTransform = transform;
}
}
public void Draw(DebugDraw drawer)
{
int i;
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
drawer.DrawLine(ref source[i], ref hit_com[i], ref green);
}
const float normalScale = 10.0f; // easier to see if this is big
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
Vector3 to = hit_surface[i] + normalScale * normal[i];
drawer.DrawLine(ref hit_surface[i], ref to, ref white);
}
Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f);
Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f);
for (i = 0; i < NUMRAYS_IN_BAR; i++)
{
Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]);
Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]);
Vector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref from, ref to, 1.0f, out linVel, out angVel);
Matrix T;
TransformUtil.IntegrateTransform(ref from, ref linVel, ref angVel, hit_fraction[i], out T);
Vector3 box1 = boxShapeHalfExtents;
Vector3 box2 = -boxShapeHalfExtents;
drawer.DrawBox(ref box1, ref box2, ref T, ref cyan);
}
}
public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
IndexedMatrix convexFromTrans = convexFromWorld;
IndexedMatrix convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
IndexedVector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
// FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i = 0; i < m_overlappingObjects.Count; i++)
{
CollisionObject collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
IndexedVector3 collisionObjectAabbMin;
IndexedVector3 collisionObjectAabbMax;
IndexedMatrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
IndexedVector3 hitNormal;
if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
{
IndexedMatrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
public void SaveKinematicState(float step)
{
//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
if (step != 0)
{
//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
if (MotionState != null)
{
MotionState.GetWorldTransform(out _worldTransform);
}
TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity);
InterpolationLinearVelocity = _linearVelocity;
InterpolationAngularVelocity = _angularVelocity;
InterpolationWorldTransform = WorldTransform;
}
}
public void Draw(DebugDraw drawer)
{
foreach (var ray in _rays)
{
drawer.DrawLine(ref ray.Source, ref ray.HitCenterOfMass, ref _green);
Vector3 to = ray.HitPoint + NormalScale * ray.Normal;
drawer.DrawLine(ref ray.HitPoint, ref to, ref _white);
Matrix fromTransform = _fromRotation * Matrix.Translation(ray.Source);
Matrix toTransform = _toRotation * Matrix.Translation(ray.Destination);
Vector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref fromTransform, ref toTransform, 1.0f, out linVel, out angVel);
Matrix transform;
TransformUtil.IntegrateTransform(ref fromTransform, ref linVel, ref angVel, ray.HitFraction, out transform);
drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, ref _cyan);
}
}
public void Draw(IDebugDraw drawer)
{
for (int i = 0; i < NumRays; i++)
{
drawer.DrawLine(ref _source[i], ref _hitCenterOfMass[i], Color.Lime);
Vector3 to = _hitPoint[i] + NormalScale * _normal[i];
drawer.DrawLine(ref _hitPoint[i], ref to, Color.White);
Matrix fromTransform = _fromRotation * Matrix.Translation(_source[i]);
Matrix toTransform = _toRotation * Matrix.Translation(_destination[i]);
Vector3 linVel, angVel;
TransformUtil.CalculateVelocity(fromTransform, toTransform, 1.0f, out linVel, out angVel);
Matrix transform;
TransformUtil.IntegrateTransform(fromTransform, linVel, angVel, _hitFraction[i], out transform);
drawer.DrawBox(ref _boxBoundMin, ref _boxBoundMax, ref transform, Color.Aqua);
}
}
public void OnMouseMove(CollisionManager CollisionManager, Camera ActiveCamera, System.Windows.Forms.MouseEventArgs e)
{
var mouse = new
{
Near = ActiveCamera.Project(new Vector2(e.X, e.Y), depth: -0.90f),
Far = ActiveCamera.Project(new Vector2(e.X, e.Y), depth: 1)
};
Matrix4 from = Matrix4.Translation(mouse.Near);
Matrix4 to = Matrix4.Translation(mouse.Far);
var d = (mouse.Far - mouse.Near).Normalized();
using (var callback = new CollisionWorld.ClosestConvexResultCallback(mouse.Near, mouse.Far))
{
callback.CollisionFilterGroup = CollisionFilterGroups.StaticFilter;
callback.CollisionFilterMask = CollisionFilterGroups.StaticFilter;
CollisionManager.World.ConvexSweepTest((ConvexShape)CollisionObject.CollisionShape, from, to, callback);
var lookingNormal = (callback.ConvexToWorld - callback.ConvexFromWorld).Normalized();
var dot = Vector3.Dot(callback.HitNormalWorld, lookingNormal);
Console.WriteLine(callback.HitNormalWorld);
if (callback.HasHit)
{
if (!this.Selected)
{
return;
}
var matrix = this.WorldMatrix.ClearTranslation();
var collMtrix = this.Model.RenderModel.compressionInfo[0].ToExtentsMatrix();
Vector3 linVel, angVel;
TransformUtil.CalculateVelocity(from, to, 1.0f, out linVel, out angVel);
Matrix4 T;
TransformUtil.IntegrateTransform(from, linVel, angVel, callback.ClosestHitFraction, out T);
this.WorldMatrix = T;
}
}
}
public void SaveKinematicState(float timeStep)
{
//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
if (timeStep != 0f)
{
//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
if (GetMotionState() != null)
{
GetMotionState().GetWorldTransform(out m_worldTransform);
}
IndexedVector3 linVel = IndexedVector3.Zero, angVel = IndexedVector3.Zero;
// debug steps to track NaN's
IndexedMatrix worldTransform = m_worldTransform;
TransformUtil.CalculateVelocity(ref m_interpolationWorldTransform, ref worldTransform, timeStep, out m_linearVelocity, out m_angularVelocity);
SetWorldTransform(ref worldTransform);
m_interpolationLinearVelocity = m_linearVelocity;
m_interpolationAngularVelocity = m_angularVelocity;
SetInterpolationWorldTransform(ref m_worldTransform);
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
}
}
public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
{
/// compute linear and angular velocity for this interval, to interpolate
IndexedVector3 linVelA, angVelA, linVelB, angVelB;
TransformUtil.CalculateVelocity(ref fromA, ref toA, 1f, out linVelA, out angVelA);
TransformUtil.CalculateVelocity(ref fromB, ref toB, 1f, out linVelB, out angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB1 != null?m_convexB1.GetAngularMotionDisc() : 0.0f;
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
IndexedVector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return(false);
}
float lambda = 0f;
IndexedVector3 v = new IndexedVector3(1, 0, 0);
int maxIter = MAX_ITERATIONS;
IndexedVector3 n = IndexedVector3.Zero;
bool hasResult = false;
IndexedVector3 c;
float lastLambda = lambda;
//float epsilon = float(0.001);
int numIter = 0;
//first solution, using GJK
float radius = 0.001f;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
{
ComputeClosestPoints(ref fromA, ref fromB, pointCollector1);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance + result.m_allowedPenetration;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
{
return(false);
}
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
IndexedVector3 colour = new IndexedVector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
float dLambda = 0f;
projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
{
return(false);
}
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f || lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return(false);
//n.setValue(0,0,0);
}
lastLambda = lambda;
//interpolate to next lambda
IndexedMatrix interpolatedTransA = IndexedMatrix.Identity, interpolatedTransB = IndexedMatrix.Identity, relativeTrans = IndexedMatrix.Identity;
TransformUtil.IntegrateTransform(ref fromA, ref linVelA, ref angVelA, lambda, out interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB, ref linVelB, ref angVelB, lambda, out interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = interpolatedTransB.InverseTimes(ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA._origin, 0.2f, new IndexedVector3(1, 0, 0));
}
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
ComputeClosestPoints(ref interpolatedTransA, ref interpolatedTransB, pointCollector);
if (pointCollector.m_hasResult)
{
dist = pointCollector.m_distance + result.m_allowedPenetration;
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
}
else
{
result.ReportFailure(-1, numIter);
return(false);
}
numIter++;
if (numIter > maxIter)
{
result.ReportFailure(-2, numIter);
return(false);
}
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return(true);
}
return(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();
}
}