// try to compute a stable ray/plane intersection. when origin is at < +/- an altitude threshold,
// use ray/line distance with view-perp line, instead of actual intersection
public static Vector3f SafeRayPlaneIntersection(Ray3f ray, Vector3f forwardDir,
Vector3f planeOrigin, Vector3f planeNormal, float fAngleThresh = 10.0f)
{
// determine if we are in unstable condition
float fOriginAngleDeg = (float)Math.Abs(
planeNormal.AngleD((planeOrigin - ray.Origin).Normalized));
bool bOriginUnstable = Math.Abs(fOriginAngleDeg - 90.0f) < fAngleThresh;
// we use ray/plane intersect if stable
Frame3f planeF = new Frame3f(planeOrigin, planeNormal);
Vector3f planeHit = planeF.RayPlaneIntersection(ray.Origin, ray.Direction, 2);
if (bOriginUnstable == false)
{
return(planeHit);
}
// if unstable, find "right" direction in plane (ie perp to forward dir, which
// may just be ray dir), then intersection is ray/axis closest point
Vector3f fwDirInPlane = (forwardDir - planeNormal.Dot(forwardDir)).Normalized;
Vector3f perpDirInPlane = Quaternionf.AxisAngleD(planeNormal, 90.0f) * fwDirInPlane;
float fAxisDist = (float)DistLine3Ray3.MinDistanceLineParam(ray, new Line3d(planeOrigin, perpDirInPlane));
return(planeOrigin + fAxisDist * perpDirInPlane);
}
public static float PlaneAngle(Vector3f a, Vector3f b, int nPlaneNormalIdx = 1)
{
a[nPlaneNormalIdx] = b[nPlaneNormalIdx] = 0.0f;
a.Normalize();
b.Normalize();
return(Vector3f.AngleD(a, b));
}
public static float PlaneAngleSigned(Vector3f vFrom, Vector3f vTo, int nPlaneNormalIdx = 1)
{
vFrom[nPlaneNormalIdx] = vTo[nPlaneNormalIdx] = 0.0f;
vFrom.Normalize();
vTo.Normalize();
float fSign = Math.Sign(Vector3f.Cross(vFrom, vTo)[nPlaneNormalIdx]);
float fAngle = fSign * Vector3f.AngleD(vFrom, vTo);
return(fAngle);
}
public static float PlaneAngleSigned(Vector3f vFrom, Vector3f vTo, Vector3f planeN)
{
vFrom = vFrom - Vector3f.Dot(vFrom, planeN) * planeN;
vTo = vTo - Vector3f.Dot(vTo, planeN) * planeN;
vFrom.Normalize();
vTo.Normalize();
Vector3f c = Vector3f.Cross(vFrom, vTo);
float fSign = Math.Sign(Vector3f.Dot(c, planeN));
float fAngle = fSign * Vector3f.AngleD(vFrom, vTo);
return(fAngle);
}
// we orbit view to be level, then again so that the normal of the hit point points towards
// the camera. Then we pull back along that direction. Unless we want to face away,
// then we rotate an extra 180 degrees, and set the target out by the pullback distance
static public void Teleport_LevelNormalOffset(fCamera camera, FScene scene,
Vector3f targetPoint, Vector3f targetNormal, float fPullback, bool bFaceAwayFromSurface = false)
{
fPullback *= scene.GetSceneScale();
Vector3f vCurCamFW = camera.GetWorldFrame().Z;
Vector3f vCurCamFWXZ = new Vector3f(vCurCamFW[0], 0.0f, vCurCamFW[2]).Normalized;
Vector3f vHitNormalXZ = new Vector3f(targetNormal[0], 0.0f, targetNormal[2]).Normalized;
float fFace = bFaceAwayFromSurface ? -1.0f : 1.0f;
float fRotate = Vector3f.AngleD(-vCurCamFWXZ, fFace * vHitNormalXZ);
float fSign = Vector3f.Cross(vCurCamFWXZ, fFace * vHitNormalXZ)[1] > 0 ? -1 : 1;
fRotate *= fSign;
// now we use cam instead of normal here because we rotated normal to face cam!
Vector3f vNewCamPos = targetPoint - fFace * fPullback * vCurCamFWXZ;
Vector3f vNewTargetPos = (bFaceAwayFromSurface) ? vNewCamPos + fPullback * vCurCamFWXZ : targetPoint;
camera.Animator().Teleport_Level(vNewCamPos, vNewTargetPos, targetPoint, fRotate);
}
public void UpdateTracking(Cockpit cockpit, fCamera camera)
{
fGameObject cockpitGO = cockpit.RootGameObject;
if (!bInitialized)
{
currentFrame = cockpit.GetLevelViewFrame(CoordSpace.WorldCoords);
currentFrame.ConstrainedAlignAxis(2, Vector3f.AxisZ, Vector3f.AxisY);
bInitialized = true;
}
Vector3f vCamFW = camera.Forward();
vCamFW[1] = 0; vCamFW.Normalize(); // I don't think this is strictly necessary but
// better to be safe for now...
Vector3f vCamPos = camera.GetPosition();
//if (tracking_debug == null)
// tracking_debug = UnityUtil.CreatePrimitiveGO("tracking_indicator", PrimitiveType.Sphere, MaterialUtil.CreateStandardMaterial(Color.green), false);
//tracking_debug.transform.position = vCamPos + 15.0f * vCamFW;
//if (tracking_avg == null) {
// tracking_avg = UnityUtil.CreatePrimitiveGO("tracking_indicator", PrimitiveType.Sphere, MaterialUtil.CreateStandardMaterial(Color.blue), false);
// tracking_avg.transform.localScale = new Vector3(0.5f, 0.5f, 0.5f);
//}
//tracking_avg.transform.position = vCamPos + 10.0f * vSlowViewDirTrack;
//tracking_debug.SetVisible(false);
//tracking_avg.SetVisible(false);
if (vSlowViewDirTrack == Vector3f.Zero)
{
vSlowViewDirTrack = vCamFW;
}
float slowTrackSpeed = 0.05f;
vSlowViewDirTrack = VRUtil.AngleLerp(vSlowViewDirTrack, vCamFW, slowTrackSpeed);
// head position tracking
if (IsLocked == false)
{
cockpitGO.SetPosition(vCamPos);
}
//Vector3 vDelta = (camera.transform.position - RootGameObject.transform.position);
//if (vDelta.magnitude > 0.2f)
// RootGameObject.transform.position = camera.transform.position;
////else if ( vDelta.magnitude > 0.05f)
//else
// RootGameObject.transform.position =
// (1.0f - TrackingSpeed) * RootGameObject.transform.position +
// (TrackingSpeed) * camera.transform.position;
float RotationSpeed = 200.0f;
float WarmupTrackingAngleThresh = 55.0f;
float ImmediateTrackingAngleThresh = 65.0f;
float StopTrackingAngleThresh = 5.0f;
float TrackingWarmupDelay = 2.0f;
float TrackingCooldownDelay = 0.75f;
//Vector3 vCockpitFW = cockpitGO.transform.forward;
Vector3f vCockpitFW = currentFrame.Z;
float fSlowHDeviation = VRUtil.PlaneAngle(vCockpitFW, vSlowViewDirTrack);
float fActualViewDeviation = VRUtil.PlaneAngle(vCockpitFW, vCamFW);
bool bDoTrack = false;
if (eState == TrackingState.NotTracking)
{
//tracking_debug.GetComponent<Renderer>().material = MaterialUtil.CreateStandardMaterial(Color.green);
if (fSlowHDeviation > WarmupTrackingAngleThresh)
{
set_tracking_state(TrackingState.TrackingWarmup);
stateChangeStartTime = FPlatform.RealTime();
}
}
else if (eState == TrackingState.TrackingWarmup)
{
//tracking_debug.GetComponent<Renderer>().material = MaterialUtil.CreateStandardMaterial(Color.yellow);
if (fSlowHDeviation > ImmediateTrackingAngleThresh)
{
set_tracking_state(TrackingState.Tracking);
}
else if (fSlowHDeviation > WarmupTrackingAngleThresh)
{
if ((FPlatform.RealTime() - stateChangeStartTime) > TrackingWarmupDelay)
{
set_tracking_state(TrackingState.Tracking);
}
}
else
{
set_tracking_state(TrackingState.NotTracking);
}
}
else if (eState == TrackingState.Tracking)
{
bDoTrack = true;
//tracking_debug.GetComponent<Renderer>().material = MaterialUtil.CreateStandardMaterial(Color.red);
if (fActualViewDeviation < StopTrackingAngleThresh)
{
set_tracking_state(TrackingState.TrackingCooldown);
stateChangeStartTime = FPlatform.RealTime();
}
}
else if (eState == TrackingState.TrackingCooldown)
{
bDoTrack = true;
//tracking_debug.GetComponent<Renderer>().material = MaterialUtil.CreateStandardMaterial(Color.gray);
if (fActualViewDeviation < StopTrackingAngleThresh)
{
if ((FPlatform.RealTime() - stateChangeStartTime) > TrackingCooldownDelay)
{
set_tracking_state(TrackingState.NotTracking);
bDoTrack = false;
}
}
else
{
set_tracking_state(TrackingState.Tracking);
}
}
if (IsLocked)
{
bDoTrack = false;
set_tracking_state(TrackingState.NotTracking);
}
if (bDoTrack)
{
float dt = (float)(FPlatform.RealTime() - animation_last_time);
float fDelta = RotationSpeed * dt;
Vector3f vCurrent = new Vector3f(vCockpitFW[0], 0, vCockpitFW[2]).Normalized;
Vector3f vTarget = new Vector3f(vSlowViewDirTrack[0], 0, vSlowViewDirTrack[2]).Normalized;
//Vector3 vTarget = new Vector3(vCamFW[0], 0, vCamFW[2]).normalized;
Vector3f c = Vector3f.Cross(vCurrent, vTarget);
float a = Vector3f.AngleD(vCurrent, vTarget);
float fSign = (c[1] < 0) ? -1.0f : 1.0f;
float fRotAngle = Math.Min(a, fDelta) * fSign;
currentFrame.Rotate(Quaternionf.AxisAngleD(Vector3f.AxisY, fRotAngle));
}
cockpitGO.SetRotation(currentFrame.Rotation);
animation_last_time = FPlatform.RealTime();
if (indicator == null)
{
indicator = new CockpitTrackingWidget();
indicator.Create(this, cockpit);
cockpit.AddUIElement(indicator, false);
}
indicator.EnableIndicator = show_indicator;
}