public void updateSprites(SInstancedSpriteData instanceData, ref RectangleF clientRect,
ref Vector3 cameraPos, ref Matrix4 camera3dView, ref Matrix4 camera3dProj)
{
var beam = _laser.beam(_beamId);
var ray = beam.rayWorld();
var laserParams = _laser.parameters;
var beamSrcInViewSpace = Vector3.Transform(beam.startPosWorld, camera3dView);
bool hideSprites = true;
Vector3 intersectPt3d;
if (beamSrcInViewSpace.Z < 0f)
{
Matrix4 cameraViewProjMat3d = camera3dView * camera3dProj;
// extract a near plane from the camera view projection matrix
// https://www.opengl.org/discussion_boards/showthread.php/159332-Extract-clip-planes-from-projection-matrix
SSPlane3d nearPlane = new SSPlane3d()
{
A = cameraViewProjMat3d.M14 + cameraViewProjMat3d.M13,
B = cameraViewProjMat3d.M24 + cameraViewProjMat3d.M23,
C = cameraViewProjMat3d.M34 + cameraViewProjMat3d.M33,
D = cameraViewProjMat3d.M44 + cameraViewProjMat3d.M43
};
if (nearPlane.intersects(ref ray, out intersectPt3d))
{
#if false
Console.WriteLine("camera at world pos = " + cameraPos);
Console.WriteLine("camera in screen pos = " + OpenTKHelper.WorldToScreen(
cameraPos, ref cameraViewProjMat3d, ref clientRect));
Console.WriteLine("screen hit at world pos = " + intersectPt3d);
#endif
float lengthToIntersectionSq =
(intersectPt3d - beam.startPosWorld).LengthSquared;
float beamLengthSq = beam.lengthSqWorld();
if (lengthToIntersectionSq < beamLengthSq)
{
hideSprites = false;
Vector2 drawScreenPos = OpenTKHelper.WorldToScreen(
intersectPt3d - ray.dir * 1f, ref cameraViewProjMat3d, ref clientRect);
//Console.WriteLine("screen hit at screen pos = " + drawScreenPos);
float intensity = _laser.envelopeIntensity * beam.periodicIntensity;
Vector2 drawScale
= new Vector2(laserParams.hitFlareSizeMaxPx * (float)Math.Exp(intensity));
for (int i = 0; i < _spriteSlotIdxs.Length; ++i)
{
int writeIdx = _spriteSlotIdxs [i];
instanceData.writePosition(writeIdx, drawScreenPos);
instanceData.writeComponentScale(writeIdx, drawScale);
instanceData.writeOrientationZ(writeIdx, intensity * 2f * (float)Math.PI);
}
Color4 backgroundColor = laserParams.backgroundColor;
backgroundColor.A = intensity;
instanceData.writeColor(_spriteSlotIdxs[(int)SpriteId.coronaBackground], backgroundColor);
Color4 overlayColor = laserParams.overlayColor;
//overlayColor.A = intensity / _laser.parameters.intensityEnvelope.sustainLevel;
overlayColor.A = Math.Min(intensity * 2f, 1f);
instanceData.writeColor(_spriteSlotIdxs[(int)SpriteId.coronaOverlay], overlayColor);
//System.Console.WriteLine("overlay.alpha == " + overlayColor.A);
Color4 ring1Color = laserParams.overlayColor;
//ring1Color.A = (float)Math.Pow(intensity, 5.0);
ring1Color.A = 0.1f * intensity;
instanceData.writeComponentScale(_spriteSlotIdxs[(int)SpriteId.ring1],
drawScale * (float)Math.Exp(intensity));
instanceData.writeColor(_spriteSlotIdxs[(int)SpriteId.ring1], ring1Color);
//instanceData.writeColor(_spriteSlotIdxs[(int)SpriteId.ring1], Color4.LimeGreen);
Color4 ring2Color = laserParams.backgroundColor;
//ring2Color.A = (float)Math.Pow(intensity, 10.0);
ring2Color.A = intensity * 0.1f;
instanceData.writeColor(_spriteSlotIdxs[(int)SpriteId.ring2], ring2Color);
//instanceData.writeColor(_spriteSlotIdxs[(int)SpriteId.ring2], Color4.Magenta);
}
}
}
if (hideSprites)
{
// hide sprites
for (int i = 0; i < _spriteSlotIdxs.Length; ++i)
{
instanceData.writeComponentScale(_spriteSlotIdxs[i], Vector2.Zero);
}
}
//System.Console.WriteLine("beam id " + _beamId + " hitting screen at xy " + hitPosOnScreen);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap;
Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 averageNormal = Vector3.Zero;
// the start
{
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position);
if (OpenTKHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
// the end
{
Vector3 oldEnd = oldCapsule.GetEnd();
Vector3 newEnd = newCapsule.GetEnd();
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd);
if (OpenTKHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldEnd - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref averageNormal);
collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
public void updateSprites(SInstancedSpriteData instanceData, ref RectangleF screenClientRect,
ref Vector3 cameraPos, ref Matrix4 camera3dView, ref Matrix4 camera3dProj)
{
float occDiskScreenAreaUsed = (float)_occDiskObj.OcclusionQueueryResult;
if (occDiskScreenAreaUsed <= 0f)
{
// hide all sprites
var nanVec = new Vector2(float.NaN);
instanceData.writePosition(_backgroundSpriteIdx, nanVec);
instanceData.writePosition(_overlaySpriteIdx, nanVec);
return;
}
var laserParams = _laser.parameters;
var beam = _laser.beam(_beamId);
float beamIntensity = _laser.envelopeIntensity * beam.periodicIntensity;
// position sprites at the beam start in screen space
Matrix4 camera3dViewProjMat = camera3dView * camera3dProj;
var beamStartScreen = OpenTKHelper.WorldToScreen(beam.startPosWorld,
ref camera3dViewProjMat, ref screenClientRect);
instanceData.writePosition(_backgroundSpriteIdx, beamStartScreen);
instanceData.writePosition(_overlaySpriteIdx, beamStartScreen);
// compute screen space needed to occupy the area where the start of the middle's crossbeam
// would be displayed
Matrix4 viewInverted = camera3dView.Inverted();
Vector3 viewRight = Vector3.Transform(Vector3.UnitX, viewInverted).Normalized();
Vector3 occRightMost = _occDiskObj.Pos + viewRight * laserParams.middleBackgroundWidth;
Vector2 occRightMostPt = OpenTKHelper.WorldToScreen(occRightMost,
ref camera3dViewProjMat, ref screenClientRect);
Vector2 occCenterPt = OpenTKHelper.WorldToScreen(_occDiskObj.Pos,
ref camera3dViewProjMat, ref screenClientRect);
float crossBeamRadiusScreenPx = Math.Abs(occRightMostPt.X - occCenterPt.X);
// write sprite size big enough to cover up the starting section of the cross beam (middle)
float scale = Math.Max(laserParams.emissionFlareScreenSizeMin, crossBeamRadiusScreenPx * 2.5f)
* beamIntensity;
instanceData.writeMasterScale(_backgroundSpriteIdx, scale * 1.2f);
instanceData.writeMasterScale(_overlaySpriteIdx, scale * 1f);
// add some variety to orientation to make the sprites look less static
instanceData.writeOrientationZ(_backgroundSpriteIdx, beamIntensity * 1f * (float)Math.PI);
instanceData.writeOrientationZ(_overlaySpriteIdx, beamIntensity * 1f * (float)Math.PI);
// color intensity: depends on the dot product between to-camera vector and beam direction;
// also depends on how of the occlusion disk area is visible
float maxScreenArea = (float)Math.PI
* laserParams.emissionOccDiskRadiusPx * laserParams.emissionOccDiskRadiusPx;
float occDiskAreaRatio = occDiskScreenAreaUsed / maxScreenArea;
//System.Console.WriteLine("occDiskAreaRatio = " + occDiskAreaRatio);
Vector3 toCamera = (cameraPos - beam.startPosWorld).Normalized();
float dot = Math.Max(0f, Vector3.Dot(toCamera, beam.directionWorld()));
//System.Console.WriteLine("dot = " + dot);
var alpha = occDiskAreaRatio * 1.2f * (float)Math.Pow(beamIntensity, 0.1) * (float)Math.Pow(dot, 0.1);
alpha = Math.Min(alpha, 1f);
// finish background color
var backgroundColor = laserParams.backgroundColor;
backgroundColor.A = alpha;
instanceData.writeColor(_backgroundSpriteIdx, backgroundColor);
// finish overlay color
var overlayColor = laserParams.overlayColor;
overlayColor.A = alpha;
instanceData.writeColor(_overlaySpriteIdx, overlayColor);
}
/// <summary> // TODO teting testing testing ...
/// Adds the forces die to this wheel to the parent. Return value indicates if it's
/// on the ground.
/// </summary>
/// <param name="dt"></param>
public bool AddForcesToCar(float dt)
{
Vector3 force = Vector3.Zero;
lastDisplacement = displacement;
displacement = 0.0f;
Body carBody = car.Chassis.Body;
Vector3 worldPos = carBody.Position + Vector3Extensions.TransformNormal(pos, carBody.Orientation); // *mPos;
Vector3 worldAxis = Vector3Extensions.TransformNormal(axisUp, carBody.Orientation); // *mAxisUp;
//Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.Orientation.GetCol(0);
// OpenGl has differnet row/column order for matrixes than XNA has ..
Vector3 wheelFwd = Vector3Extensions.TransformNormal(carBody.Orientation.Right(), JiggleMath.RotationMatrix(steerAngle, worldAxis));
//Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.GetOrientation().GetCol(0);
Vector3 wheelUp = worldAxis;
Vector3 wheelLeft = Vector3.Cross(wheelUp, wheelFwd);
wheelLeft.Normalize();
wheelUp = Vector3.Cross(wheelFwd, wheelLeft);
// start of ray
float rayLen = 2.0f * radius + travel;
Vector3 wheelRayEnd = worldPos - radius * worldAxis;
Segment wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis);
//Assert(PhysicsSystem.CurrentPhysicsSystem);
CollisionSystem collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem;
//Assert(collSystem);
int numRaysUse = System.Math.Min(numRays, maxNumRays);
// adjust the start position of the ray - divide the wheel into numRays+2
// rays, but don't use the first/last.
float deltaFwd = (2.0f * radius) / (numRaysUse + 1);
float deltaFwdStart = deltaFwd;
lastOnFloor = false;
int bestIRay = 0;
int iRay;
for (iRay = 0; iRay < numRaysUse; ++iRay)
{
fracs[iRay] = float.MaxValue; //SCALAR_HUGE;
// work out the offset relative to the middle ray
float distFwd = (deltaFwdStart + iRay * deltaFwd) - radius;
//float zOffset = mRadius * (1.0f - CosDeg(90.0f * (distFwd / mRadius)));
float zOffset = radius * (1.0f - (float)System.Math.Cos(OpenTKHelper.ToRadians(90.0f * (distFwd / radius))));
segments[iRay] = wheelRay;
segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp;
if (collSystem.SegmentIntersect(out fracs[iRay], out otherSkins[iRay],
out groundPositions[iRay], out groundNormals[iRay], segments[iRay], pred))
{
lastOnFloor = true;
if (fracs[iRay] < fracs[bestIRay])
{
bestIRay = iRay;
}
}
}
if (!lastOnFloor)
{
return(false);
}
//Assert(bestIRay < numRays);
// use the best one
Vector3 groundPos = groundPositions[bestIRay];
float frac = fracs[bestIRay];
CollisionSkin otherSkin = otherSkins[bestIRay];
// const Vector3 groundNormal = (worldPos - segments[bestIRay].GetEnd()).NormaliseSafe();
// const Vector3 groundNormal = groundNormals[bestIRay];
Vector3 groundNormal = worldAxis;
if (numRaysUse > 1)
{
for (iRay = 0; iRay < numRaysUse; ++iRay)
{
if (fracs[iRay] <= 1.0f)
{
groundNormal += (1.0f - fracs[iRay]) * (worldPos - segments[iRay].GetEnd());
}
}
JiggleMath.NormalizeSafe(ref groundNormal);
}
else
{
groundNormal = groundNormals[bestIRay];
}
//Assert(otherSkin);
Body worldBody = otherSkin.Owner;
displacement = rayLen * (1.0f - frac);
displacement = OpenTKHelper.Clamp(displacement, 0, travel);
float displacementForceMag = displacement * spring;
// reduce force when suspension is par to ground
displacementForceMag *= Vector3.Dot(groundNormals[bestIRay], worldAxis);
// apply damping
float dampingForceMag = upSpeed * damping;
float totalForceMag = displacementForceMag + dampingForceMag;
if (totalForceMag < 0.0f)
{
totalForceMag = 0.0f;
}
Vector3 extraForce = totalForceMag * worldAxis;
force += extraForce;
// side-slip friction and drive force. Work out wheel- and floor-relative coordinate frame
Vector3 groundUp = groundNormal;
Vector3 groundLeft = Vector3.Cross(groundNormal, wheelFwd);
JiggleMath.NormalizeSafe(ref groundLeft);
Vector3 groundFwd = Vector3.Cross(groundLeft, groundUp);
Vector3 wheelPointVel = carBody.Velocity +
Vector3.Cross(carBody.AngularVelocity, Vector3Extensions.TransformNormal(pos, carBody.Orientation));// * mPos);
Vector3 rimVel = angVel * Vector3.Cross(wheelLeft, groundPos - worldPos);
wheelPointVel += rimVel;
// if sitting on another body then adjust for its velocity.
if (worldBody != null)
{
Vector3 worldVel = worldBody.Velocity +
Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position);
wheelPointVel -= worldVel;
}
// sideways forces
float noslipVel = 0.2f;
float slipVel = 0.4f;
float slipFactor = 0.7f;
float smallVel = 3;
float friction = sideFriction;
float sideVel = Vector3.Dot(wheelPointVel, groundLeft);
if ((sideVel > slipVel) || (sideVel < -slipVel))
{
friction *= slipFactor;
}
else
if ((sideVel > noslipVel) || (sideVel < -noslipVel))
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel);
}
if (sideVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(sideVel) < smallVel)
{
friction *= System.Math.Abs(sideVel) / smallVel;
}
float sideForce = -friction * totalForceMag;
extraForce = sideForce * groundLeft;
force += extraForce;
// fwd/back forces
friction = fwdFriction;
float fwdVel = Vector3.Dot(wheelPointVel, groundFwd);
if ((fwdVel > slipVel) || (fwdVel < -slipVel))
{
friction *= slipFactor;
}
else
if ((fwdVel > noslipVel) || (fwdVel < -noslipVel))
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel);
}
if (fwdVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(fwdVel) < smallVel)
{
friction *= System.Math.Abs(fwdVel) / smallVel;
}
float fwdForce = -friction * totalForceMag;
extraForce = fwdForce * groundFwd;
force += extraForce;
//if (!force.IsSensible())
//{
// TRACE_FILE_IF(ONCE_1)
// TRACE("Bad force in car wheel\n");
// return true;
//}
// fwd force also spins the wheel
Vector3 wheelCentreVel = carBody.Velocity +
Vector3.Cross(carBody.AngularVelocity, Vector3Extensions.TransformNormal(pos, carBody.Orientation));// * mPos);
angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / radius;
torque += -fwdForce * radius;
// add force to car
carBody.AddWorldForce(force, groundPos);
//if (float.IsNaN(force.X))
// while(true){}
//System.Diagnostics.Debug.WriteLine(force.ToString());
// add force to the world
if (worldBody != null && !worldBody.Immovable)
{
// todo get the position in the right place...
// also limit the velocity that this force can produce by looking at the
// mass/inertia of the other object
float maxOtherBodyAcc = 500.0f;
float maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass;
if (force.LengthSquared > (maxOtherBodyForce * maxOtherBodyForce))
{
force *= maxOtherBodyForce / force.Length;
}
worldBody.AddWorldForce(-force, groundPos);
}
return(true);
}
/// <summary>
/// Apply
/// </summary>
/// <param name="dt"></param>
/// <returns>bool</returns>
public override bool Apply(float dt)
{
this.Satisfied = true;
bool body0FrozenPre = !body0.IsActive;
bool body1FrozenPre = !body1.IsActive;
if (body0FrozenPre && body1FrozenPre)
{
return(false);
}
#region REFERENCE: Vector3 currentVel0 = body0.Velocity + Vector3.Cross(body0.AngVel, R0);
Vector3 currentVel0 = body0.AngularVelocity;
Vector3.Cross(ref currentVel0, ref R0, out currentVel0);
Vector3.Add(ref body0.transformRate.Velocity, ref currentVel0, out currentVel0);
#endregion
#region REFERENCE: Vector3 currentVel1 = body1.Velocity + Vector3.Cross(body1.AngVel, R1);
Vector3 currentVel1 = body1.AngularVelocity;
Vector3.Cross(ref currentVel1, ref R1, out currentVel1);
Vector3.Add(ref body1.transformRate.Velocity, ref currentVel1, out currentVel1);
#endregion
// predict a new location
#region REFERENCE: Vector3 predRelPos0 = (currentRelPos0 + (currentVel0 - currentVel1) * dt);
Vector3 predRelPos0;
Vector3.Subtract(ref currentVel0, ref currentVel1, out predRelPos0);
Vector3.Multiply(ref predRelPos0, dt, out predRelPos0);
Vector3.Add(ref predRelPos0, ref currentRelPos0, out predRelPos0);
#endregion
// if the new position is out of range then clamp it
Vector3 clampedRelPos0 = predRelPos0;
float clampedRelPos0Mag = clampedRelPos0.Length;
if (clampedRelPos0Mag <= JiggleMath.Epsilon)
{
return(false);
}
if (clampedRelPos0Mag > mMaxDistance)
#region REFERENCE: clampedRelPos0 *= mMaxDistance / clampedRelPos0Mag;
{
Vector3.Multiply(ref clampedRelPos0, mMaxDistance / clampedRelPos0Mag, out clampedRelPos0);
}
#endregion
// now claculate desired vel based on the current pos, new/clamped
// pos and dt
#region REFERENCE: Vector3 desiredRelVel0 = ((clampedRelPos0 - currentRelPos0) / System.Math.Max(dt, JiggleMath.Epsilon));
Vector3 desiredRelVel0;
Vector3.Subtract(ref clampedRelPos0, ref currentRelPos0, out desiredRelVel0);
Vector3.Divide(ref desiredRelVel0, OpenTKHelper.Max(dt, JiggleMath.Epsilon), out desiredRelVel0);
#endregion
// Vr is -ve the total velocity change
#region REFERENCE: Vector3 Vr = (currentVel0 - currentVel1) - desiredRelVel0;
Vector3 Vr;
Vector3.Subtract(ref currentVel0, ref currentVel1, out Vr);
Vector3.Subtract(ref Vr, ref desiredRelVel0, out Vr);
#endregion
float normalVel = Vr.Length;
// limit it
if (normalVel > maxVelMag)
{
#region REFERENCE: Vr *= (maxVelMag / normalVel);
Vector3.Multiply(ref Vr, maxVelMag / normalVel, out Vr);
#endregion
normalVel = maxVelMag;
}
else if (normalVel < minVelForProcessing)
{
return(false);
}
#region REFERENCE: Vector3 N = Vr / normalVel;
Vector3 N;
Vector3.Divide(ref Vr, normalVel, out N);
#endregion
#region REFERENCE: float denominator = body0.InvMass + body1.InvMass + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R0, N), body0.WorldInvInertia), R0)) + Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(R1, N), body1.WorldInvInertia), R1));
Vector3 v1; float f1, f2;
Vector3.Cross(ref R0, ref N, out v1);
Vector3Extensions.TransformNormal(ref v1, ref body0.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref R0, out v1);
Vector3.Dot(ref N, ref v1, out f1);
Vector3.Cross(ref R1, ref N, out v1);
Vector3Extensions.TransformNormal(ref v1, ref body1.worldInvInertia, out v1);
Vector3.Cross(ref v1, ref R1, out v1);
Vector3.Dot(ref N, ref v1, out f2);
float denominator = body0.InverseMass + body1.InverseMass + f1 + f2;
#endregion
if (denominator < JiggleMath.Epsilon)
{
return(false);
}
float normalImpulse = -normalVel / denominator;
#region REFERENCE: if (!body0.Immovable) body0.ApplyWorldImpulse(normalImpulse * N, worldPos);
Vector3 imp;
Vector3.Multiply(ref N, normalImpulse, out imp);
if (!body0.Immovable)
{
body0.ApplyWorldImpulse(ref imp, ref worldPos);
}
#endregion
#region REFERENCE: if (!body1.Immovable) body1.ApplyWorldImpulse(-normalImpulse * N, worldPos);
Vector3.Multiply(ref N, -normalImpulse, out imp);
if (!body1.Immovable)
{
body1.ApplyWorldImpulse(ref imp, ref worldPos);
}
#endregion
body0.SetConstraintsAndCollisionsUnsatisfied();
body1.SetConstraintsAndCollisionsUnsatisfied();
this.Satisfied = true;
return(true);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
JigLibX.Geometry.Plane oldPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
JigLibX.Geometry.Plane newPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
Matrix4 newPlaneInvTransform = newPlane.InverseTransformMatrix;
Matrix4 oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
// the start
{
Vector3 oldCapsuleStartPos = Vector3.Transform(oldCapsule.Position, oldPlaneInvTransform);
Vector3 newCapsuleStartPos = Vector3.Transform(newCapsule.Position, newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleStartPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleStartPos, newPlane);
if (OpenTKHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position8(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * oldPlane.Normal;
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
}
// the end
{
Vector3 oldCapsuleEndPos = Vector3.Transform(oldCapsule.GetEnd(), oldPlaneInvTransform);
Vector3 newCapsuleEndPos = Vector3.Transform(newCapsule.GetEnd(), newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleEndPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleEndPos, newPlane);
if (System.Math.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.GetEnd() - oldCapsule.Radius * oldPlane.Normal;
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts);
}
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
public static void SimpleShadowmapProjection(
List <SSObject> objects,
SSLight light,
FrustumCuller frustum, // can be null (disabled)
SSCamera camera,
out float width, out float height, out float nearZ, out float farZ,
out Vector3 viewEye, out Vector3 viewTarget, out Vector3 viewUp)
{
if (light.Type != SSLight.LightType.Directional)
{
throw new NotSupportedException();
}
// light-aligned unit vectors
Vector3 lightZ = light.Direction.Normalized();
Vector3 lightX, lightY;
OpenTKHelper.TwoPerpAxes(lightZ, out lightX, out lightY);
// Step 1: light-direction aligned AABB of the visible objects
Vector3 projBBMin = new Vector3(float.PositiveInfinity);
Vector3 projBBMax = new Vector3(float.NegativeInfinity);
foreach (var obj in objects)
{
if (obj.renderState.toBeDeleted ||
!obj.renderState.visible ||
!obj.renderState.castsShadow)
{
continue;
}
else if (frustum == null || (obj.boundingSphere != null &&
frustum.isSphereInsideFrustum(obj.Pos, obj.ScaledRadius)))
{
// determine AABB in light coordinates of the objects so far
Vector3 lightAlignedPos = OpenTKHelper.ProjectCoord(obj.Pos, lightX, lightY, lightZ);
Vector3 rad = new Vector3(obj.ScaledRadius);
Vector3 localMin = lightAlignedPos - rad;
Vector3 localMax = lightAlignedPos + rad;
projBBMin = Vector3.ComponentMin(projBBMin, localMin);
projBBMax = Vector3.ComponentMax(projBBMax, localMax);
}
}
if (frustum != null)
{
// then we need to do a second pass, including shadow-casters that
// are between the camera-frusum and the light
// compute the camera's position in lightspace, because we need to
// include everything "closer" that the midline of the camera frustum
Vector3 lightAlignedCameraPos = OpenTKHelper.ProjectCoord(camera.Pos, lightX, lightY, lightZ);
float minZTest = lightAlignedCameraPos.Z;
// TODO what happens if all objects are exluded?
// Step 2: Extend Z of AABB to cover objects "between" current AABB and the light
foreach (var obj in objects)
{
if (obj.renderState.toBeDeleted ||
!obj.renderState.visible ||
!obj.renderState.castsShadow)
{
continue;
}
Vector3 lightAlignedPos = OpenTKHelper.ProjectCoord(obj.Pos, lightX, lightY, lightZ);
Vector3 rad = new Vector3(obj.ScaledRadius);
Vector3 localMin = lightAlignedPos - rad;
Vector3 localMax = lightAlignedPos + rad;
if (OpenTKHelper.RectsOverlap(projBBMin.Xy, projBBMax.Xy, localMin.Xy, localMax.Xy) &&
localMin.Z < minZTest)
{
projBBMin = Vector3.ComponentMin(projBBMin, localMin);
projBBMax = Vector3.ComponentMax(projBBMax, localMax);
}
}
}
// Finish the projection matrix
// Use center of AABB in regular coordinates to get the view matrix
Vector3 centerAligned = (projBBMin + projBBMax) / 2f;
viewTarget = centerAligned.X * lightX
+ centerAligned.Y * lightY
+ centerAligned.Z * lightZ;
float farEnough = (centerAligned.Z - projBBMin.Z) + 1f;
viewEye = viewTarget - farEnough * lightZ;
viewUp = lightY;
width = projBBMax.X - projBBMin.X;
height = projBBMax.Y - projBBMin.Y;
nearZ = 1f;
farZ = 1f + (projBBMax.Z - projBBMin.Z);
}
public void updateExecution(float timeElapsed)
{
if (timeElapsed <= 0f)
{
return;
}
var mParams = _missile.cluster.parameters;
var target = _missile.cluster.target;
// basic proportional navigation. see wikipedia
Vector3 Vr = target.velocity - _missile.velocity;
Vector3 R = target.position - _missile.position;
Vector3 omega = Vector3.Cross(R, Vr) / R.LengthSquared;
Vector3 latax = mParams.pursuitNavigationGain * Vector3.Cross(Vr, omega);
if (mParams.pursuitAugmentedPN == true)
{
// this code is not tested as there are currently no targets with well defined accelerations
Vector3 losDir = R.Normalized();
float targetAccLos = Vector3.Dot(target.acceleration, losDir);
Vector3 targetLatAx = target.acceleration - targetAccLos * losDir;
latax += mParams.pursuitNavigationGain * targetLatAx / 2f;
}
_missile._lataxDebug = latax;
// apply latax
var oldVelMag = _missile.velocity.LengthFast;
_missile.velocity += latax * timeElapsed;
float tempVelMag = _missile.velocity.LengthFast;
if (oldVelMag != 0f)
{
float r = tempVelMag / oldVelMag;
if (r > 1f)
{
_missile.velocity /= r;
}
}
if (mParams.pursuitHitTimeCorrection)
{
// apply pursuit hit time correction
float dist = R.LengthFast;
if (dist != 0f)
{
Vector3 targetDir = R / dist;
float v0 = -Vector3.Dot(Vr, targetDir);
float t = _missile.cluster.timeToHit;
float correctionAccMag = 2f * (dist - v0 * t) / t / t;
Vector3 corrAcc = correctionAccMag * targetDir;
_missile.velocity += corrAcc * timeElapsed;
_missile._hitTimeCorrAccDebug = corrAcc;
}
}
else
{
// hit time correction inactive. allow accelerating to achieve optimal velocity or forever
oldVelMag = _missile.velocity.LengthFast;
float velDelta = mParams.pursuitMaxAcc * timeElapsed;
float newVelMag = Math.Min(oldVelMag + velDelta, mParams.pursuitMaxVelocity);
if (oldVelMag != 0f)
{
_missile.velocity *= (newVelMag / oldVelMag);
}
}
// make visual direction "lean into" velocity
Vector3 axis;
float angle;
OpenTKHelper.neededRotation(_missile.visualDirection, _missile.velocity.Normalized(),
out axis, out angle);
float abs = Math.Abs(angle);
if (abs > mParams.pursuitVisualRotationRate && abs > 0f)
{
angle = angle / abs * mParams.pursuitVisualRotationRate;
}
Quaternion quat = Quaternion.FromAxisAngle(axis, angle);
_missile.visualDirection = Vector3.Transform(_missile.visualDirection, quat);
}
/// <summary>
/// Detect BoxBox Collisions.
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Box box0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
Box box1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
Box oldBox0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box oldBox1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Matrix4 dirs0 = box0.Orientation;
Matrix4 dirs1 = box1.Orientation;
Vector3 box0_Right = dirs0.Right();
Vector3 box0_Up = dirs0.Up();
Vector3 box0_Backward = dirs0.Backward();
Vector3 box1_Right = dirs1.Right();
Vector3 box1_Up = dirs1.Up();
Vector3 box1_Backward = dirs1.Backward();
float testDepth;
if (Disjoint(out testDepth, ref box0_Right, box0, box1, collTolerance))
{
return;
}
float depth = testDepth;
Vector3 N = box0_Right;
int minAxis = 0;
if (Disjoint(out testDepth, ref box0_Up, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box0_Up;
minAxis = 1;
}
if (Disjoint(out testDepth, ref box0_Backward, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box0_Backward;
minAxis = 2;
}
if (Disjoint(out testDepth, ref box1_Right, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box1_Right;
minAxis = 3;
}
if (Disjoint(out testDepth, ref box1_Up, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box1_Up;
minAxis = 4;
}
if (Disjoint(out testDepth, ref box1_Backward, box0, box1, collTolerance))
{
return;
}
if (testDepth < depth)
{
depth = testDepth;
N = box1_Backward;
minAxis = 5;
}
Vector3 axis;
Vector3.Cross(ref box0_Right, ref box1_Right, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 6;
}
Vector3.Cross(ref box0_Right, ref box1_Up, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 7;
}
Vector3.Cross(ref box0_Right, ref box1_Backward, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 8;
}
Vector3.Cross(ref box0_Up, ref box1_Right, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 9;
}
Vector3.Cross(ref box0_Up, ref box1_Up, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 10;
}
Vector3.Cross(ref box0_Up, ref box1_Backward, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 11;
}
Vector3.Cross(ref box0_Backward, ref box1_Right, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 12;
}
Vector3.Cross(ref box0_Backward, ref box1_Up, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 13;
}
Vector3.Cross(ref box0_Backward, ref box1_Backward, out axis);
if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
{
return;
}
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
minAxis = 14;
}
Vector3 D = box1.GetCentre() - box0.GetCentre();
N.Normalize();
int i;
/*seperatingAxes[0] = dirs0.Right;
* seperatingAxes[1] = dirs0.Up;
* seperatingAxes[2] = dirs0.Backward;
* seperatingAxes[3] = dirs1.Right;
* seperatingAxes[4] = dirs1.Up;
* seperatingAxes[5] = dirs1.Backward;
* Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]);
* Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]);
* Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]);
* Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]);
* Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]);
* Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]);
* Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]);
* Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]);
* Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]);
*
*
* // see if the boxes are separate along any axis, and if not keep a
* // record of the depths along each axis
* int i;
* for (i = 0; i < 15; ++i)
* {
* // If we can't normalise the axis, skip it
* float l2 = seperatingAxes[i].LengthSquared;
*
* if (l2 < JiggleMath.Epsilon) continue;
*
* overlapDepth[i] = float.MaxValue;
*
* if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, collTolerance))
* return;
* }
*
* // The box overlap, find the seperation depth closest to 0.
* float minDepth = float.MaxValue;
* int minAxis = -1;
*
* for (i = 0; i < 15; ++i)
* {
* // If we can't normalise the axis, skip it
* float l2 = seperatingAxes[i].LengthSquared;
* if (l2 < JiggleMath.Epsilon) continue;
*
* // Normalise the separation axis and depth
* float invl = 1.0f / (float)System.Math.Sqrt(l2);
* seperatingAxes[i] *= invl;
* overlapDepth[i] *= invl;
*
* // If this axis is the minmum, select it
* if (overlapDepth[i] < minDepth)
* {
* minDepth = overlapDepth[i];
* minAxis = i;
* }
* }
*
* if (minAxis == -1)
* return;
*
* // Make sure the axis is facing towards the 0th box.
* // if not, invert it
* Vector3 D = box1.GetCentre() - box0.GetCentre();
* Vector3 N = seperatingAxes[minAxis];
* float depth = overlapDepth[minAxis];*/
if (Vector3.Dot(D, N) > 0.0f)
{
N *= -1.0f;
}
float minA = OpenTKHelper.Min(box0.SideLengths.X, OpenTKHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z));
float minB = OpenTKHelper.Min(box1.SideLengths.X, OpenTKHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z));
float combinationDist = 0.05f * OpenTKHelper.Min(minA, minB);
// the contact points
bool contactPointsFromOld = true;
contactPts.Clear();
if (depth > -JiggleMath.Epsilon)
{
GetBoxBoxIntersectionPoints(contactPts, oldBox0, oldBox1, combinationDist, collTolerance);
}
int numPts = contactPts.Count;
if (numPts == 0)
{
contactPointsFromOld = false;
GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, collTolerance);
}
numPts = contactPts.Count;
Vector3 body0OldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1OldPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
Vector3 body0NewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 body1NewPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.Position : Vector3.Zero;
#region REFERENCE: Vector3 bodyDelta = body0NewPos - body0OldPos - body1NewPos + body1OldPos;
Vector3 bodyDelta;
Vector3.Subtract(ref body0NewPos, ref body0OldPos, out bodyDelta);
Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta);
Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta);
#endregion
#region REFERENCE: float bodyDeltaLen = Vector3.Dot(bodyDelta,N);
float bodyDeltaLen;
Vector3.Dot(ref bodyDelta, ref N, out bodyDeltaLen);
#endregion
float oldDepth = depth + bodyDeltaLen;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 SATPoint;
switch (minAxis)
{
// Box0 face, Box1 corner collision
case 0:
case 1:
case 2:
{
// Get the lowest point on the box1 along box1 normal
GetSupportPoint(out SATPoint, box1, -N);
break;
}
// We have a Box2 corner/Box1 face collision
case 3:
case 4:
case 5:
{
// Find with vertex on the triangle collided
GetSupportPoint(out SATPoint, box0, N);
break;
}
// We have an edge/edge collision
default:
/*case 6:
* case 7:
* case 8:
* case 9:
* case 10:
* case 11:
* case 12:
* case 13:
* case 14:*/
{
{
// Retrieve which edges collided.
i = minAxis - 6;
int ia = i / 3;
int ib = i - ia * 3;
// find two P0, P1 point on both edges.
Vector3 P0, P1;
GetSupportPoint(out P0, box0, N);
GetSupportPoint(out P1, box1, -N);
// Find the edge intersection.
// plane along N and F, and passing through PB
Vector3 box0Orient, box1Orient;
JiggleUnsafe.Get(ref box0.transform.Orientation, ia, out box0Orient);
JiggleUnsafe.Get(ref box1.transform.Orientation, ib, out box1Orient);
#region REFERENCE: Vector3 planeNormal = Vector3.Cross(N, box1Orient[ib]);
Vector3 planeNormal;
Vector3.Cross(ref N, ref box1Orient, out planeNormal);
#endregion
#region REFERENCE: float planeD = Vector3.Dot(planeNormal, P1);
float planeD;
Vector3.Dot(ref planeNormal, ref P1, out planeD);
#endregion
// find the intersection t, where Pintersection = P0 + t*box edge dir
#region REFERENCE: float div = Vector3.Dot(box0Orient, planeNormal);
float div;
Vector3.Dot(ref box0Orient, ref planeNormal, out div);
#endregion
// plane and ray colinear, skip the intersection.
if (System.Math.Abs(div) < JiggleMath.Epsilon)
{
return;
}
float t = (planeD - Vector3.Dot(P0, planeNormal)) / div;
// point on edge of box0
#region REFERENCE: P0 += box0Orient * t;
P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0);
#endregion
#region REFERENCE: SATPoint = (P0 + (0.5f * depth) * N);
Vector3.Multiply(ref N, 0.5f * depth, out SATPoint);
Vector3.Add(ref SATPoint, ref P0, out SATPoint);
#endregion
}
break;
}
/*default:
* throw new Exception("Impossible switch");*/
}
// distribute the depth according to the distance to the SAT point
if (numPts > 0)
{
float minDist = float.MaxValue;
float maxDist = float.MinValue;
for (i = 0; i < numPts; ++i)
{
float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
if (dist < minDist)
{
minDist = dist;
}
if (dist > maxDist)
{
maxDist = dist;
}
}
if (maxDist < minDist + JiggleMath.Epsilon)
{
maxDist = minDist + JiggleMath.Epsilon;
}
// got some intersection points
for (i = 0; i < numPts; ++i)
{
float minDepthScale = 0.0f;
float dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
float depthDiv = System.Math.Max(JiggleMath.Epsilon, maxDist - minDist);
float depthScale = (dist - minDist) / depthDiv;
depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth;
if (contactPointsFromOld)
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Instead of allocating a new SmallCollPointInfo we reuse the existing one.
collPts[numCollPts].R0 = contactPts[i].Pos - body0OldPos;
collPts[numCollPts].R1 = contactPts[i].Pos - body1OldPos;
collPts[numCollPts++].InitialPenetration = depth;
}
}
else
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Instead of allocating a new SmallCollPointInfo we reuse the existing one.
collPts[numCollPts].R0 = contactPts[i].Pos - body0NewPos;
collPts[numCollPts].R1 = contactPts[i].Pos - body1NewPos;
collPts[numCollPts++].InitialPenetration = depth;
}
}
}
}
else
{
#region REFERENCE: collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
//collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
Vector3 cp0;
Vector3.Subtract(ref SATPoint, ref body0NewPos, out cp0);
Vector3 cp1;
Vector3.Subtract(ref SATPoint, ref body1NewPos, out cp1);
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Instead of allocating a new SmallCollPointInfo we reuse the existing one.
collPts[numCollPts].R0 = cp0;
collPts[numCollPts].R1 = cp1;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
#endregion
}
// report Collisions
collisionFunctor.CollisionNotify(ref info, ref N, collPts, numCollPts);
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
public void updateSprites(SInstancedSpriteData instanceData, ref RectangleF clientRect,
ref Vector3 cameraPos, ref Matrix4 camera3dView, ref Matrix4 camera3dProj)
{
Matrix4 camera3dViewProjMat = camera3dView * camera3dProj;
if (_sunDiskOccObj != null)
{
Matrix4 viewInverted = _sunDiskOccScene.renderConfig.invCameraViewMatrix.Inverted();
Vector3 viewRight = Vector3.Transform(Vector3.UnitX, viewInverted).Normalized();
Vector3 viewUp = Vector3.Transform(Vector3.UnitY, viewInverted).Normalized();
_sunDiskOccPos = OpenTKHelper.WorldToScreen(_sunDiskOccObj.Pos, ref camera3dViewProjMat, ref clientRect);
float bbFullEstimate;
// note that it is assumed that the sun object is fully symmertircal when it is in view
if (_sunDiskOccObj.renderState.matchScaleToScreenPixels)
{
_sunDiskScreenSize = 2f * _sunDiskOccObj.Scale.Xy;
bbFullEstimate = (float)Math.PI * _sunDiskOccObj.Scale.X * _sunDiskOccObj.Scale.Y;
}
else
{
Vector3 occRightMost = _sunDiskOccObj.Pos + viewRight * _sunDiskOccObj.Scale.X;
Vector3 occTopMost = _sunDiskOccObj.Pos + viewUp * _sunDiskOccObj.Scale.Y;
Vector2 occRightMostPt = OpenTKHelper.WorldToScreen(occRightMost, ref camera3dViewProjMat, ref clientRect);
Vector2 occTopMostPt = OpenTKHelper.WorldToScreen(occTopMost, ref camera3dViewProjMat, ref clientRect);
_sunDiskScreenSize = 2f * new Vector2(occRightMostPt.X - _sunDiskOccPos.X, _sunDiskOccPos.Y - occTopMostPt.Y);
bbFullEstimate = (float)Math.PI * (float)_sunDiskScreenSize.X * (float)_sunDiskScreenSize.Y / 4f;
}
_sunDiskOccIntensity = Math.Min((float)_sunDiskOccObj.OcclusionQueueryResult / bbFullEstimate, 1f);
}
int numElements = _spriteSlotIdxs.Length;
if (_sunDiskOccIntensity <= 0f)
{
// hide all sprites
for (int i = 0; i < numElements; ++i)
{
instanceData.writePosition(_spriteSlotIdxs [i], new Vector2(float.NaN)); // hide all sprites
}
}
else
{
Vector2 compScale = new Vector2(Math.Max(_sunDiskScreenSize.X, _sunDiskScreenSize.Y)
* Math.Min(1.5f, 1f / (1f - _sunDiskOccIntensity)));
Vector2 center = new Vector2(clientRect.X, clientRect.Y)
+ new Vector2(clientRect.Width, clientRect.Height) / 2f;
Vector2 towardsCenter = center - _sunDiskOccPos;
var color4 = _sunDiskOccObj.MainColor;
color4.A = _sunDiskOccIntensity;
for (int i = 0; i < numElements; ++i)
{
int writeIdx = _spriteSlotIdxs [i];
instanceData.writeComponentScale(writeIdx, compScale);
instanceData.writeColor(writeIdx, color4);
Vector2 spriteCenter = _sunDiskOccPos + towardsCenter * 2.5f
/ (float)numElements * (float)i;
instanceData.writePosition(_spriteSlotIdxs[i], spriteCenter);
}
}
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
// get the skins in the order that we're expecting
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere;
Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere;
Capsule oldCapsule = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule;
Capsule newCapsule = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule;
Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward());
Segment newSeg = new Segment(oldCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward());
float radSum = newCapsule.Radius + newSphere.Radius;
float oldt, newt;
float oldDistSq = Distance.PointSegmentDistanceSq(out oldt, oldSphere.Position, oldSeg);
float newDistSq = Distance.PointSegmentDistanceSq(out newt, newSphere.Position, newSeg);
if (OpenTKHelper.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance))
{
Vector3 segPos = oldSeg.GetPoint(oldt);
Vector3 delta = oldSphere.Position - segPos;
float dist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radSum - dist;
if (dist > JiggleMath.Epsilon)
{
delta /= dist;
}
else
{
// todo - make this not random
delta = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
Vector3 worldPos = segPos +
((oldCapsule.Radius - 0.5f * depth) * delta);
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos,
worldPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1);
}
}
}
/// <summary>
/// Update stuff at the end of physics
/// </summary>
/// <param name="dt"></param>
public void PostPhysics(float dt)
{
for (int i = 0; i < wheels.Count; i++)
{
wheels[i].Update(dt);
}
// control inputs
float deltaAccelerate = dt * 4.0f;
float deltaSteering = dt * steerRate;
// update the actual values
float dAccelerate = destAccelerate - accelerate;
dAccelerate = OpenTKHelper.Clamp(dAccelerate, -deltaAccelerate, deltaAccelerate);
accelerate += dAccelerate;
float dSteering = destSteering - steering;
dSteering = OpenTKHelper.Clamp(dSteering, -deltaSteering, deltaSteering);
steering += dSteering;
// apply these inputs
float maxTorque = driveTorque;
if (fWDrive && bWDrive)
{
maxTorque *= 0.5f;
}
if (fWDrive)
{
wheels[(int)WheelId.WheelFL].AddTorque(maxTorque * accelerate);
wheels[(int)WheelId.WheelFR].AddTorque(maxTorque * accelerate);
}
if (bWDrive)
{
wheels[(int)WheelId.WheelBL].AddTorque(maxTorque * accelerate);
wheels[(int)WheelId.WheelBR].AddTorque(maxTorque * accelerate);
}
wheels[(int)WheelId.WheelBL].Lock = (hBrake > 0.5f);
wheels[(int)WheelId.WheelBR].Lock = (hBrake > 0.5f);
// steering angle applies to the inner wheel. The outer one needs to match it
int inner, outer;
if (steering > 0.0f)
{
inner = (int)WheelId.WheelFL;
outer = (int)WheelId.WheelFR;
}
else
{
inner = (int)WheelId.WheelFR;
outer = (int)WheelId.WheelFL;
}
float alpha = System.Math.Abs(maxSteerAngle * steering);
float angleSgn = steering > 0.0f ? 1.0f : -1.0f;
wheels[inner].SteerAngle = (angleSgn * alpha);
float beta;
if (alpha == 0.0f)
{
beta = alpha;
}
else
{
float dx = (wheels[(int)WheelId.WheelFR].Pos.X - wheels[(int)WheelId.WheelBR].Pos.X);
float dy = (wheels[(int)WheelId.WheelFL].Pos.Z - wheels[(int)WheelId.WheelFR].Pos.Z);
//beta = ATan2Deg(dy, dx + (dy / TanDeg(alpha)));
beta = (float)System.Math.Atan2(OpenTKHelper.ToRadians(dy), OpenTKHelper.ToRadians(dx + (dy / (float)System.Math.Tan(OpenTKHelper.ToRadians(alpha)))));
beta = OpenTKHelper.ToDegrees(beta);
}
wheels[outer].SteerAngle = (angleSgn * beta);
}
protected virtual void setupScene()
{
scene = new SSScene(mainShader, pssmShader, instancingShader, instancingPssmShader);
sunDiskScene = new SSScene();
sunFlareScene = new SSScene();
hudScene = new SSScene();
environmentScene = new SSScene();
scene.renderConfig.frustumCulling = true; // TODO: fix the frustum math, since it seems to be broken.
scene.BeforeRenderObject += beforeRenderObjectHandler;
// 0. Add Lights
var light = new SSDirectionalLight(LightName.Light0);
light.Direction = new Vector3(0f, 0f, -1f);
#if true
if (OpenTKHelper.areFramebuffersSupported())
{
if (scene.renderConfig.pssmShader != null && scene.renderConfig.instancePssmShader != null)
{
light.ShadowMap = new SSParallelSplitShadowMap(TextureUnit.Texture7);
}
else
{
light.ShadowMap = new SSSimpleShadowMap(TextureUnit.Texture7);
}
}
if (!light.ShadowMap.IsValid)
{
light.ShadowMap = null;
}
#endif
scene.AddLight(light);
#if true
var smapDebug = new SSObjectHUDQuad(light.ShadowMap.TextureID);
smapDebug.Scale = new Vector3(0.3f);
smapDebug.Pos = new Vector3(50f, 200, 0f);
hudScene.AddObject(smapDebug);
#endif
// setup a sun billboard object and a sun flare spriter renderer
{
var sunDisk = new SSMeshDisk();
var sunBillboard = new SSObjectBillboard(sunDisk, true);
sunBillboard.MainColor = new Color4(1f, 1f, 0.8f, 1f);
sunBillboard.Pos = new Vector3(0f, 0f, 18000f);
sunBillboard.Scale = new Vector3(600f);
sunBillboard.renderState.frustumCulling = false;
sunBillboard.renderState.lighted = false;
sunBillboard.renderState.castsShadow = false;
sunDiskScene.AddObject(sunBillboard);
SSTexture flareTex = SSAssetManager.GetInstance <SSTextureWithAlpha>(".", "sun_flare.png");
const float bigOffset = 0.8889f;
const float smallOffset = 0.125f;
RectangleF[] flareSpriteRects =
{
new RectangleF(0f, 0f, 1f, bigOffset),
new RectangleF(0f, bigOffset, smallOffset, smallOffset),
new RectangleF(smallOffset, bigOffset, smallOffset, smallOffset),
new RectangleF(smallOffset * 2f, bigOffset, smallOffset, smallOffset),
new RectangleF(smallOffset * 3f, bigOffset, smallOffset, smallOffset),
};
float[] spriteScales = { 20f, 1f, 2f, 1f, 1f };
var sunFlare = new SimpleSunFlareMesh(sunDiskScene, sunBillboard, flareTex,
flareSpriteRects, spriteScales);
sunFlare.Scale = new Vector3(2f);
sunFlare.renderState.lighted = false;
sunFlareScene.AddObject(sunFlare);
}
}
/// <summary>
/// Assumes dir is normalised. Angle is in degrees.
/// </summary>
/// <param name="ang"></param>
/// <param name="dir"></param>
/// <returns>Matrix4</returns>
public static Matrix4 RotationMatrix(float ang, Vector3 dir)
{
return(Matrix4.CreateFromAxisAngle(dir, OpenTKHelper.ToRadians(ang)));
}
public List <ssBVHNode <GO> > traverse(SSRay ray)
{
float tnear = 0f, tfar = 0f;
return(traverse(box => OpenTKHelper.intersectRayAABox1(ray, box, ref tnear, ref tfar)));
}
/// <summary>
/// Detect BoxPlane Collisions.
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
JPlane oldPlane = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as JPlane;
JPlane newPlane = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as JPlane;
Matrix4 newPlaneInvTransform = newPlane.InverseTransformMatrix;
Vector3 newBoxCen = Vector3.Transform(newBox.GetCentre(), newPlaneInvTransform);
// quick check
float centreDist = Distance.PointPlaneDistance(newBoxCen, newPlane);
if (centreDist > collTolerance + newBox.GetBoundingRadiusAroundCentre())
{
return;
}
Matrix4 oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
Vector3[] newPts;
newBox.GetCornerPoints(out newPts);
Vector3[] oldPts;
oldBox.GetCornerPoints(out oldPts);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
for (int i = 0; i < 8; ++i)
{
Vector3.Transform(ref oldPts[i], ref oldPlaneInvTransform, out oldTransPts[i]);
Vector3.Transform(ref newPts[i], ref newPlaneInvTransform, out newPts[i]);
float oldDepth = -Distance.PointPlaneDistance(ref oldTransPts[i], oldPlane);
float newDepth = -Distance.PointPlaneDistance(ref newPts[i], newPlane);
if (OpenTKHelper.Max(oldDepth, newDepth) > -collTolerance)
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses instead of reallocating.
collPts[numCollPts].R0 = oldPts[i] - body0Pos;
collPts[numCollPts].R1 = oldPts[i] - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
}
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
protected void moveShips(float timeElapsed)
{
if (timeElapsed <= 0f)
{
return;
}
// make the target drone move from side to side
localTime += timeElapsed;
Vector3 pos = targetDrone.Pos;
pos.Z = 30f * (float)Math.Sin(localTime);
targetDrone.Pos = pos;
// make the vandal ship orbit missile target
Vector3 desiredPos;
Vector3 desiredDir;
float angle = localTime * 0.5f;
float desiredXOffset = 100f * (float)Math.Cos(angle);
float desiredYOffset = 20f * (float)Math.Sin(angle * 0.77f);
float desiredZOffset = 80f * (float)Math.Sin(angle * 0.88f);
Vector3 desiredOffset = new Vector3(desiredXOffset, desiredYOffset, desiredZOffset);
var target = getTargetObject();
if (missileLauncher != MissileLaunchers.VandalShip || target == null || target == vandalShip)
{
desiredPos = new Vector3(100f, 0f, 0f);
desiredDir = -Vector3.UnitX;
}
else if (target == main3dScene.ActiveCamera)
{
desiredPos = main3dScene.ActiveCamera.Pos + -main3dScene.ActiveCamera.Dir * 300f;
Quaternion cameraOrient = OpenTKHelper.neededRotation(Vector3.UnitZ, -main3dScene.ActiveCamera.Up);
desiredPos += Vector3.Transform(desiredOffset * 0.1f, cameraOrient);
desiredDir = (target.Pos - vandalShip.Pos).Normalized();
}
else
{
//float desiredZOffset = 5f * (float)Math.Sin(angle + 0.2f);
desiredPos = target.Pos + desiredOffset;
desiredDir = (target.Pos - vandalShip.Pos).Normalized();
}
Vector3 desiredMotion = desiredPos - vandalShip.Pos;
const float vel = 100f;
float displacement = vel * timeElapsed;
Vector3 vandalNewPos;
if (displacement > desiredMotion.LengthFast)
{
vandalNewPos = desiredPos;
}
else
{
vandalNewPos = vandalShip.Pos + desiredMotion.Normalized() * displacement;
}
vandalVelocity = (vandalNewPos - vandalShip.Pos) / timeElapsed;
vandalShip.Pos = vandalNewPos;
Quaternion vandalOrient = OpenTKHelper.neededRotation(Vector3.UnitZ, desiredDir);
vandalShip.Orient(desiredDir, Vector3.Transform(Vector3.UnitY, vandalOrient));
}
/// <summary>
/// Initialise
/// </summary>
/// <param name="body0"></param>
/// <param name="body1"></param>
/// <param name="hingeAxis"></param>
/// <param name="hingePosRel0"></param>
/// <param name="hingeHalfWidth"></param>
/// <param name="hingeFwdAngle"></param>
/// <param name="hingeBckAngle"></param>
/// <param name="sidewaysSlack"></param>
/// <param name="damping"></param>
public void Initialise(Body body0, Body body1, Vector3 hingeAxis, Vector3 hingePosRel0,
float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping)
{
this.body0 = body0;
this.body1 = body1;
this.hingeAxis = hingeAxis;
this.hingePosRel0 = hingePosRel0;
this.usingLimit = false;
this.damping = damping;
// tScalar allowedDistance = 0.005f;
this.hingeAxis.Normalize();
Vector3 hingePosRel1 = body0.Position + hingePosRel0 - body1.Position;
// generate the two positions relative to each body
Vector3 relPos0a = hingePosRel0 + hingeHalfWidth * hingeAxis;
Vector3 relPos0b = hingePosRel0 - hingeHalfWidth * hingeAxis;
Vector3 relPos1a = hingePosRel1 + hingeHalfWidth * hingeAxis;
Vector3 relPos1b = hingePosRel1 - hingeHalfWidth * hingeAxis;
float timescale = 1.0f / 20.0f;
float allowedDistanceMid = 0.005f;
float allowedDistanceSide = sidewaysSlack * hingeHalfWidth;
mSidePointConstraints = new ConstraintMaxDistance[2];
mSidePointConstraints[0] = new ConstraintMaxDistance();
mSidePointConstraints[1] = new ConstraintMaxDistance();
mSidePointConstraints[0].Initialise(body0, relPos0a, body1, relPos1a, allowedDistanceSide);
mSidePointConstraints[1].Initialise(body0, relPos0b, body1, relPos1b, allowedDistanceSide);
mMidPointConstraint = new ConstraintPoint();
mMidPointConstraint.Initialise(body0, hingePosRel0, body1, hingePosRel1, allowedDistanceMid, timescale);
if (hingeFwdAngle <= 150) // MAX_HINGE_ANGLE_LIMIT
{
// choose a direction that is perpendicular to the hinge
Vector3 perpDir = Vector3Extensions.Up;
if (Vector3.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = Vector3Extensions.Right;
}
// now make it perpendicular to the hinge
Vector3 sideAxis = Vector3.Cross(hingeAxis, perpDir);
perpDir = Vector3.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * hingeHalfWidth;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
Vector3 hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
Vector3 hingeRelAnchorPos1 = Vector3Extensions.TransformNormal(hingeRelAnchorPos0, Matrix4.CreateFromAxisAngle(hingeAxis, OpenTKHelper.ToRadians(-angleToMiddle)));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(OpenTKHelper.ToRadians(hingeHalfAngle * 0.5f));
Vector3 hingePos = body1.Position + hingePosRel0;
Vector3 relPos0c = hingePos + hingeRelAnchorPos0 - body0.Position;
Vector3 relPos1c = hingePos + hingeRelAnchorPos1 - body1.Position;
mMaxDistanceConstraint = new ConstraintMaxDistance();
mMaxDistanceConstraint.Initialise(body0, relPos0c, body1, relPos1c, allowedDistance);
usingLimit = true;
}
if (damping <= 0.0f)
{
damping = -1.0f; // just make sure that a value of 0.0 doesn't mess up...
}
else
{
damping = OpenTKHelper.Clamp(damping, 0, 1);
}
}
/// <summary>
/// GetHeightAndNormal
/// </summary>
/// <param name="h"></param>
/// <param name="normal"></param>
/// <param name="point"></param>
public void GetHeightAndNormal(out float h, out Vector3 normal, Vector3 point)
{
float x = point.X;
float z = point.Z;
x = OpenTKHelper.Clamp(x, xMin, xMax);
z = OpenTKHelper.Clamp(z, zMin, zMax);
int i0 = (int)((x - xMin) / dx);
int j0 = (int)((point.Z - zMin) / dz);
i0 = (int)OpenTKHelper.Clamp((int)i0, 0, mHeights.Nx - 1);
j0 = (int)OpenTKHelper.Clamp((int)j0, 0, mHeights.Nz - 1);
int i1 = i0 + 1;
int j1 = j0 + 1;
if (i1 >= (int)mHeights.Nx)
{
i1 = mHeights.Nx - 1;
}
if (j1 >= (int)mHeights.Nz)
{
j1 = mHeights.Nz - 1;
}
float iFrac = (x - (i0 * dx + xMin)) / dx;
float jFrac = (z - (j0 * dz + zMin)) / dz;
iFrac = OpenTKHelper.Clamp(iFrac, 0.0f, 1.0f);
jFrac = OpenTKHelper.Clamp(jFrac, 0.0f, 1.0f);
float h00 = mHeights[i0, j0];
float h01 = mHeights[i0, j1];
float h10 = mHeights[i1, j0];
float h11 = mHeights[i1, j1];
// All the triangles are orientated the same way.
// work out the normal, then z is in the plane of this normal
if ((i0 == i1) && (j0 == j1))
{
normal = Vector3Extensions.Up;
}
else if (i0 == i1)
{
Vector3 right = Vector3Extensions.Right;
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), right);
normal.Normalize();
}
if (j0 == j1)
{
Vector3 backw = Vector3Extensions.Backward;
normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else if (iFrac > jFrac)
{
normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else
{
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz));
normal.Normalize();
}
// get the plane equation
// h00 is in all the triangles
JiggleMath.NormalizeSafe(ref normal);
Vector3 pos = new Vector3((i0 * dx + xMin), h00, (j0 * dz + zMin));
float d; Vector3.Dot(ref normal, ref pos, out d); d = -d;
h = Distance.PointPlaneDistance(ref point, ref normal, d);
}
public void preRenderUpdate(float timeElapsed)
{
bool visible = (targetObj != null);
_outline.renderState.visible = visible;
_labelBelow.renderState.visible = visible;
_labelAbove.renderState.visible = visible;
if (!visible)
{
return;
}
RectangleF clientRect = OpenTKHelper.GetClientRect();
var targetRc = _targetObj3dScene.renderConfig;
Matrix4 targetViewProj = targetRc.invCameraViewMatrix * targetRc.projectionMatrix;
// outline
Quaternion viewRotOnly = targetRc.invCameraViewMatrix.ExtractRotation();
Quaternion viewRotInverted = viewRotOnly.Inverted();
Vector3 viewRight = Vector3.Transform(Vector3.UnitX, viewRotInverted).Normalized();
Vector3 viewUp = Vector3.Transform(Vector3.UnitY, viewRotInverted).Normalized();
Vector2 targetScreenPos = OpenTKHelper.WorldToScreen(targetObj.Pos, ref targetViewProj, ref clientRect);
targetScreenPos.X = (float)Math.Round(targetScreenPos.X); // fixes off-by-one pixel jitter
targetScreenPos.Y = (float)Math.Round(targetScreenPos.Y);
// animate outline line stipple
_outline.enableLineStipple = this.isSelected;
if (_outline.enableLineStipple)
{
ushort stipplePattern = _outline.lineStipplePattern;
_stippleTimeAccumulator += timeElapsed;
while (_stippleTimeAccumulator >= stippleStepInterval)
{
ushort firstBit = (ushort)((uint)stipplePattern & 0x1);
stipplePattern >>= 1;
stipplePattern |= (ushort)((uint)firstBit << 15);
_outline.lineStipplePattern = stipplePattern;
_stippleTimeAccumulator -= stippleStepInterval;
}
}
float outlineHalfWidth;
float outlineHalfHeight;
if (fixedSizeTargets)
{
outlineHalfWidth = outlineMinPixelSz;
outlineHalfHeight = outlineMinPixelSz;
}
else
{
// assumes target is a convential SSObject without billboarding, match scale to screen, etc.
var size = targetObj.worldBoundingSphereRadius;
Vector3 targetRightMost = targetObj.Pos + viewRight * size;
Vector3 targetTopMost = targetObj.Pos + viewUp * size;
Vector2 screenRightMostPt = OpenTKHelper.WorldToScreen(targetRightMost, ref targetViewProj, ref clientRect);
Vector2 screenTopMostPt = OpenTKHelper.WorldToScreen(targetTopMost, ref targetViewProj, ref clientRect);
outlineHalfWidth = 2f * (screenRightMostPt.X - targetScreenPos.X);
outlineHalfWidth = Math.Max(outlineHalfWidth, outlineMinPixelSz);
outlineHalfHeight = 2f * (targetScreenPos.Y - screenTopMostPt.Y);
outlineHalfHeight = Math.Max(outlineHalfHeight, outlineMinPixelSz);
}
Vector3 targetViewPos = Vector3.Transform(targetObj.Pos, targetRc.invCameraViewMatrix);
_targetViewDepth = targetViewPos.Z;
bool targetIsInFront = _targetViewDepth < 0f;
float lineWidth = targetIsInFront ? outlineWidthWhenInFront : outlinelineWidthWhenBehind;
bool above, below, left, right;
if (targetIsInFront)
{
left = targetScreenPos.X + outlineHalfWidth < 0f;
right = !left && targetScreenPos.X - outlineHalfWidth > clientRect.Width;
above = targetScreenPos.Y + outlineHalfHeight < 0f;
below = !above && targetScreenPos.Y + outlineHalfHeight > clientRect.Height;
}
else // target is behind
{
float halfScrWidth = clientRect.Width / 2f;
float halfScrHeight = clientRect.Height / 2f;
float quartScrWidth = halfScrWidth / 2f;
float quartScrHeight = halfScrHeight / 2f;
right = targetScreenPos.X < quartScrWidth;
left = !right && targetScreenPos.X > halfScrWidth + quartScrWidth;
below = targetScreenPos.Y < quartScrHeight;
above = !below && targetScreenPos.Y > halfScrHeight + quartScrHeight;
}
int orientIdx = (above ? 1 : 0) + (below ? 2 : 0) + (left ? 4 : 0) + (right ? 8 : 0);
bool inTheCenter = (orientIdx == 0);
if (!inTheCenter)
{
outlineHalfWidth = outlineMinPixelSz;
outlineHalfHeight = outlineMinPixelSz;
if (left)
{
targetScreenPos.X = outlineHalfWidth;
}
else if (right)
{
targetScreenPos.X = clientRect.Width - outlineHalfWidth - lineWidth * 2f;
}
if (above)
{
targetScreenPos.Y = outlineHalfHeight + _labelAbove.getGdiSize.Height;
}
else if (below)
{
targetScreenPos.Y = clientRect.Height - outlineHalfHeight - _labelBelow.getGdiSize.Height;
}
}
_outline.Mesh = inTheCenter ? (targetIsInFront ? hudRectLinesMesh : hudCircleMesh)
: hudTriMesh;
_outline.Scale = new Vector3(outlineHalfWidth, outlineHalfHeight, 1f);
_outline.Orient(outlineOrients [orientIdx]);
_outline.Pos = new Vector3(targetScreenPos.X, targetScreenPos.Y, +1f);
// labels
_labelBelow.Label = fetchTextBelow(targetObj);
var labelBelowPos = targetScreenPos;
if (left)
{
labelBelowPos.X = 0f;
}
else if (right)
{
labelBelowPos.X = clientRect.Width - _labelBelow.getGdiSize.Width - 10f;
}
else
{
labelBelowPos.X -= _labelBelow.getGdiSize.Width / 2f;
}
labelBelowPos.Y += outlineHalfHeight;
if ((left || right) && !below)
{
labelBelowPos.Y += outlineHalfHeight;
}
_labelBelow.Pos = new Vector3(labelBelowPos.X, labelBelowPos.Y, 0f);
_labelAbove.Label = fetchTextAbove(targetObj);
var labelAbovePos = targetScreenPos;
if (left)
{
labelAbovePos.X = 0f;
}
else if (right)
{
labelAbovePos.X = clientRect.Width - _labelAbove.getGdiSize.Width - 10f;
}
else
{
labelAbovePos.X -= _labelAbove.getGdiSize.Width / 2f;
}
if ((left || right) && !above)
{
labelAbovePos.Y -= outlineHalfHeight;
}
labelAbovePos.Y -= (outlineHalfHeight + _labelAbove.getGdiSize.Height);
_labelAbove.Pos = new Vector3(labelAbovePos.X, labelAbovePos.Y, 0f);
Color4 color = fetchColor(targetObj);
_outline.MainColor = color;
_labelBelow.MainColor = color;
_labelAbove.MainColor = color;
_outlineScreenRect = new Rectangle(
(int)(targetScreenPos.X - outlineHalfWidth),
(int)(targetScreenPos.Y - outlineHalfHeight),
(int)(outlineHalfWidth * 2f),
(int)(outlineHalfHeight * 2f)
);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
// get the skins in the order that we're expectiing
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward());
Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward());
float radius = oldCapsule.Radius;
Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
float oldSegT;
float oldBoxT0, oldBoxT1, oldBoxT2;
float oldDistSq = Distance.SegmentBoxDistanceSq(out oldSegT, out oldBoxT0, out oldBoxT1, out oldBoxT2, oldSeg, oldBox);
float newSegT;
float newBoxT0, newBoxT1, newBoxT2;
float newDistSq = Distance.SegmentBoxDistanceSq(out newSegT, out newBoxT0, out newBoxT1, out newBoxT2, newSeg, newBox);
if (OpenTKHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance)))
{
Vector3 segPos = oldSeg.GetPoint(oldSegT);
Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right() +
oldBoxT1 * oldBox.Orientation.Up() + oldBoxT2 * oldBox.Orientation.Backward();
float dist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radius - dist;
Vector3 dir;
if (dist > JiggleMath.Epsilon)
{
dir = segPos - boxPos;
JiggleMath.NormalizeSafe(ref dir);
}
else if ((segPos - oldBox.GetCentre()).LengthSquared > JiggleMath.Epsilon)
{
dir = segPos - oldBox.GetCentre();
JiggleMath.NormalizeSafe(ref dir);
}
else
{
// todo - make this not random
dir = Vector3.Transform(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1);
}
}
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Sphere oldSphere0 = (Sphere)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Sphere newSphere0 = (Sphere)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
Sphere oldSphere1 = (Sphere)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
Sphere newSphere1 = (Sphere)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
Vector3 oldDelta = oldSphere0.Position - oldSphere1.Position;
Vector3 newDelta = newSphere0.Position - oldSphere1.Position;
float oldDistSq = oldDelta.LengthSquared;
float newDistSq = newDelta.LengthSquared;
float radSum = newSphere0.Radius + newSphere1.Radius;
if (System.Math.Min(oldDistSq, newDistSq) < ((radSum + collTolerance) * (radSum + collTolerance)))
{
float oldDist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radSum - oldDist;
if (oldDist > JiggleMath.Epsilon)
{
oldDelta /= oldDist;
}
else
{
// TODO - make this not random...!
oldDelta = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
Vector3 worldPos = oldSphere1.Position +
(oldSphere1.Radius - 0.5f * depth) * oldDelta;
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref oldDelta, &collInfo, 1);
}
}
}
