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); } } }