/// <summary> /// Updates benmarker system with algorithm rotating. /// </summary> void BenmarkerUpdate_AlgorithmRotating() { Vector3 rawPos = Vector3.zero; Quaternion rawRot = Quaternion.identity; m_SubMarkerMask = 0; if (!TagTrackingUtil.GetMarkerState(this.MarkerID, out rawPos, out rawRot, out m_SubMarkerMask)) { return; } if (m_FixedQuaternionError) { rawRot = rawRot * Quaternion.Euler(-90, 0, 0); } var trackingAnchor = TagTrackingUtil.GetNode(UnityEngine.XR.XRNode.TrackingReference); var head = TagTrackingUtil.GetNode(UnityEngine.XR.XRNode.CenterEye); if (this.SmoothMarkerRotation) { Quaternion rawRot1 = default(Quaternion); switch (this.smoothRotationMethod) { case SmoothRotationMethod.Lerp: rawRot1 = Quaternion.Lerp(prevRawQ, rawRot, this.SmoothRotationLerpRate); break; case SmoothRotationMethod.Angular: rawRot1 = Quaternion.RotateTowards(prevRawQ, rawRot, this.SmoothRotationAnglarSpeed * Time.deltaTime); break; } prevRawQ = rawRot; rawRot = rawRot1; } else { prevRawQ = rawRot; } Matrix4x4 markerWorldTRS = Matrix4x4.TRS(trackingAnchor.TransformPoint(rawPos), trackingAnchor.rotation * rawRot, Vector3.one).inverse; var headInMarkerWorldXYZ = markerWorldTRS.MultiplyPoint3x4(head.position); var headInMarkerWorldEuler = markerWorldTRS.rotation * head.rotation; if (this.PositionPrediction) { headInMarkerWorldXYZ = this.XYZPrediction.GetPrediction(headInMarkerWorldXYZ); } if (this.RotationPrediciton) { this.PosePrediction.Update(headInMarkerWorldEuler); headInMarkerWorldEuler = this.PosePrediction.GetRotationPrediction(headInMarkerWorldEuler, this.RotationPredictionDelayFrame); } var markerTrans = this.transform; head.position = markerTrans.TransformPoint(headInMarkerWorldXYZ); head.rotation = markerTrans.rotation * headInMarkerWorldEuler; }
protected void UpdateMarkerPose() { //raw pos : the tracked position retrieved from VPU Vector3 rawPos = Vector3.zero; //raw rot : the tracked oritentation retrieved from VPU Quaternion rawRot = Quaternion.identity; var markerTrans = this.transform; m_SubMarkerMask = 0; if (TagTrackingUtil.GetMarkerState(this.MarkerID, out rawPos, out rawRot, out this.m_SubMarkerMask)) { //Anti-jiggering: markers become invisible when TOooo far: if (this.HasLimitedTrackingDistance && rawPos.magnitude >= MaxTrackingDistance) { prevRawPosition = rawPos; prevRawOritentation = rawRot; prevWorldPosition = transform.position; //too far this.Identity.IsVisible = false; return; } Vector3 rawTrackedPosition = rawPos; this.Identity.IsVisible = true; var trackingAnchor = TagTrackingUtil.GetNode(UnityEngine.XR.XRNode.TrackingReference); //smooth XYZ movement: if (this.SmoothMarkerPosition) { switch (this.smoothPositionMethod) { case SmoothPositionMethod.Lerp: rawTrackedPosition = Vector3.Lerp(prevRawPosition, rawPos, this.PositionSmoothLerp); break; case SmoothPositionMethod.Velocity: rawTrackedPosition = Vector3.MoveTowards(prevRawPosition, rawPos, this.PositionSmoothVelocity * Time.deltaTime); break; case SmoothPositionMethod.LerpByDistance: float distance = Vector3.Distance(rawPos, prevRawPosition); float lerpByDistance = this.SmoothPositionCurveControl_LerpByDistance.Evaluate(distance); rawTrackedPosition = Vector3.Lerp(prevRawPosition, rawPos, lerpByDistance); break; case SmoothPositionMethod.VelocityByDistance: distance = Vector3.Distance(rawPos, prevRawPosition); float velocityByDistance = this.SmoothPositionCurveControl_LerpByDistance.Evaluate(distance); rawTrackedPosition = Vector3.MoveTowards(prevRawPosition, rawPos, velocityByDistance * Time.deltaTime); break; } } //basic tracking: Vector3 tPos = trackingAnchor.TransformPoint(rawTrackedPosition); //prediction: if (this.PositionPrediction) { tPos = this.XYZPrediction.GetPrediction(tPos); } markerTrans.position = tPos; //Handle Rotation: if (this.ApplyAlgorithmRotation) { Quaternion RawTrackedOritentation = rawRot; if (this.SmoothMarkerRotation) { switch (this.smoothRotationMethod) { case SmoothRotationMethod.Lerp: RawTrackedOritentation = Quaternion.Lerp(prevRawOritentation, rawRot, this.SmoothRotationLerpRate); break; case SmoothRotationMethod.Angular: RawTrackedOritentation = Quaternion.RotateTowards(prevRawOritentation, rawRot, this.SmoothRotationAnglarSpeed * Time.deltaTime); break; case SmoothRotationMethod.LerpByDistance: float distance = rawPos.magnitude; float lerp = this.SmoothRotationCurveControl_LerpByDistance.Evaluate(distance); RawTrackedOritentation = Quaternion.Lerp(prevRawOritentation, rawRot, lerp); break; case SmoothRotationMethod.AngularByDistance: distance = rawPos.magnitude; float angular = this.SmoothRotationCurveControl_AngularByDistance.Evaluate(distance); RawTrackedOritentation = Quaternion.RotateTowards(prevRawOritentation, rawRot, angular * Time.deltaTime); break; } } Quaternion worldQ = trackingAnchor.rotation * RawTrackedOritentation; if (this.RotationPrediciton) { this.PosePrediction.Update(worldQ); worldQ = this.PosePrediction.GetRotationPrediction(worldQ, this.RotationPredictionDelayFrame); } markerTrans.rotation = worldQ; } prevRawPosition = rawPos; prevRawOritentation = rawRot; prevWorldPosition = transform.position; prevWorldRotation = transform.rotation; } else { this.Identity.IsVisible = false; } }