IEnumerator _Coro_CenterFishGenerateAndMove(int circleIdx) { float elapse = 0F; float rotateSpeed = 6.283185F / OneCircleTime;//弧度/s float useTime = OneCircleTime * mRunRound + CenterFishRunoutTime[circleIdx]; Fish fishCenter = Instantiate(Prefabs_FishCenter[circleIdx]) as Fish; Swimmer centerSwimmer = fishCenter.swimmer; fishCenter.ClearAI(); fishCenter.transform.parent = transform; fishCenter.transform.localPosition = new Vector3(Locals_Center[circleIdx].x, Locals_Center[circleIdx].y, Locals_Center[circleIdx].z - 0.1F); fishCenter.transform.localRotation = Quaternion.identity; centerSwimmer.RotateSpd = 6.283185F; while (elapse < useTime) { if (centerSwimmer == null || fishCenter == null || !fishCenter.Attackable)//暂时用attackable来确定鱼死亡 { break; } //中间鱼转动 centerSwimmer.transform.localRotation = Quaternion.AxisAngle(Vector3.forward, centerSwimmer.RotateSpd); centerSwimmer.RotateSpd -= rotateSpeed * Time.deltaTime; elapse += Time.deltaTime; yield return(0); } if (centerSwimmer != null) { centerSwimmer.Speed = FishRunoutSpeed * 0.7F; centerSwimmer.Go(); } }
void Update() { if (axes == RotationAxes.MouseXAndY) { // Read the mouse input axis rotationX += Input.GetAxis("Mouse X") * sensitivityX; rotationY += Input.GetAxis("Mouse Y") * sensitivityY; rotationX = ClampAngle(rotationX, minimumX, maximumX); rotationY = ClampAngle(rotationY, minimumY, maximumY); Quaternion xQuaternion = Quaternion.AxisAngle(Vector3.up, Mathf.Deg2Rad * rotationX); Quaternion yQuaternion = Quaternion.AxisAngle(Vector3.left, Mathf.Deg2Rad * rotationY); transform.localRotation = originalRotation * xQuaternion * yQuaternion; } else if (axes == RotationAxes.MouseX) { rotationX += Input.GetAxis("Mouse X") * sensitivityX; rotationX = ClampAngle(rotationX, minimumX, maximumX); Quaternion xQuaternion = Quaternion.AxisAngle(Vector3.up, Mathf.Deg2Rad * rotationX); transform.localRotation = originalRotation * xQuaternion; } else { rotationY += Input.GetAxis("Mouse Y") * sensitivityY; rotationY = ClampAngle(rotationY, minimumY, maximumY); Quaternion yQuaternion = Quaternion.AxisAngle(Vector3.left, Mathf.Deg2Rad * rotationY); transform.localRotation = originalRotation * yQuaternion; } }
void CmdAttack(Vector3 spawnPos, Vector3 playerPos) { GameObject go = Instantiate(currentWeapon, spawnPos, Quaternion.identity); if (go.CompareTag("MeleeWeapon")) { go.GetComponent <Rigidbody2D>().velocity = Vector3.zero; Destroy(go, 0.5f); } else { go.GetComponent <Rigidbody2D>().velocity = (spawnPos - playerPos) * Time.deltaTime * bulletSpeed * 200; Destroy(go, 10f); } NetworkServer.Spawn(go); if (currentWeapon == S686) // three bullets per shoot { go = Instantiate(currentWeapon, spawnPos, Quaternion.AxisAngle(new Vector3(0, 0, 1), 0.1f)); go.GetComponent <Rigidbody2D>().velocity = (spawnPos - playerPos) * Time.deltaTime * bulletSpeed * 200; Destroy(go, 10f); NetworkServer.Spawn(go); go = Instantiate(currentWeapon, spawnPos, Quaternion.AxisAngle(new Vector3(0, 0, 1), -0.1f)); go.GetComponent <Rigidbody2D>().velocity = (spawnPos - playerPos) * Time.deltaTime * bulletSpeed * 200; Destroy(go, 10f); NetworkServer.Spawn(go); } }
void DrawLine() { float distance = PlayerController.instance.distanceToPlayer2; //float scaleY; //if (distance > 2) // scaleY = 50 / distance + 5; //else scaleY = 25; Vector3 P1 = PlayerController.instance.transform.position; Vector3 P2 = Player2Controller.instance.transform.position; float deltaX = P2.x - P1.x; float deltaY = P2.y - P1.y; float rotateZ = Mathf.Atan(deltaY / deltaX); line.transform.rotation = Quaternion.AxisAngle(new Vector3(0, 0, 1), rotateZ); line.transform.localScale = new Vector3(distance, 5, 1); line.transform.position = (P2 + P1) / 2; //Debug.Log(distance); //Debug.Log(line.GetComponent<SpriteRenderer>().color.a); if (distance > 5) { line.GetComponent <SpriteRenderer>().color = new Color(255, 255, 0, 0); } else { line.GetComponent <SpriteRenderer>().color = new Color(255, 255 / distance, 0, 255 - 51 * distance); } //Debug.Log(line.GetComponent<SpriteRenderer>().color); }
void FixedUpdate() { float h; float v; if (controllerName == "Keyboard") { h = Input.GetAxisRaw("Horizontal"); v = Input.GetAxisRaw("Vertical"); Turning(); } else { h = Input.GetAxisRaw(controllerName + "LStickX"); if (Mathf.Abs(h) < 0.2) { h = 0; } v = Input.GetAxisRaw(controllerName + "LStickY"); if (Mathf.Abs(v) < 0.2) { v = 0; } if ((Mathf.Abs(Input.GetAxisRaw(controllerName + "RStickX")) > 0.2) || (Mathf.Abs(Input.GetAxisRaw(controllerName + "RStickY")) > 0.2)) { Quaternion stickAngle = Quaternion.AxisAngle(rotationAxe, Mathf.Atan2(Input.GetAxisRaw(controllerName + "RStickX"), Input.GetAxisRaw(controllerName + "RStickY"))); Turning(stickAngle); } //Move(); } Move(h, v); Animating(h, v); }
public void OrderedUpdate() { targetPosition = target.TransformPoint(offset); Vector3 targetOffset = targetPosition - position; Vector3 smoothVelocity = targetOffset * stiffness; smoothVelocity -= velocity * damping; velocity += smoothVelocity; position += velocity; // if (Vector3.Distance(position, target.position) > maxDistance){ // Vector3 newPosition = (position - target.position).normalized * maxDistance; // velocity = newPosition - position; // } transform.position = position; // Rotation // Get the angle between the current direction and the velocity direction float offsetAngle = Vector3.Angle(target.up, up); float sign = Mathf.Sign(Vector3.Cross(up, target.up).z); offsetAngle *= sign * angularStiffness; offsetAngle -= angularVelocity * angularDamping; angularVelocity += offsetAngle; angle += angularVelocity; // up = Vector3.RotateTowards(up, target.up, angularVelocity, 0.0f); Quaternion rotation = Quaternion.AxisAngle(Vector3.forward, angle); // Apply the rotation over time transform.rotation = rotation; up = transform.up; // targetRotation = target.rotation; // Quaternion rotationOffset = targetRotation * Quaternion.Inverse(rotation); // Quaternion smoothRotation = Quaternion.Slerp( // Quaternion.identity, // rotationOffset, // angularStiffness // ); // smoothRotation = Quaternion.Normalize(smoothRotation); // smoothRotation *= Quaternion.Slerp( // Quaternion.identity, Quaternion.Inverse(angularVelocity), // angularDamping // ); // smoothRotation = Quaternion.Normalize(smoothRotation); // angularVelocity *= smoothRotation; // rotation *= angularVelocity; // transform.rotation = rotation; }
/// Like Quaternion.AngleAxis, but takes angle in radians. public static Quaternion AngleAxisRad(float angle, Vector3 axis) { // Versions that use radians are called "AxisAngle" rather than "AngleAxis". // They're marked deprecated because Unity wants everyone to use degrees. #pragma warning disable 0612, 0618 return(Quaternion.AxisAngle(axis, angle)); #pragma warning restore 0612, 0618 }
public void start(Camera camera) { direction = camera.direction; Position = camera.Position + direction * 0.5; rot = Quaternion.AxisAngle(direction, Math.PI / 30); RotationMatrix = camera.currentRotationMatrix; //RotationMatrix = Matrix4x4.Identity; timeSpan = 0; activated = true; }
IEnumerator _Coro_CenterFishGenerateAndMove(int circleIdx) { var main = GameMain.Singleton; if (main == null) { yield break; } var fishGenerator = main.FishGenerator; if (fishGenerator == null) { yield break; } var fishIndex = CenterFishIndexs[circleIdx]; var prefabFish = fishGenerator.GetFishPrefab(fishIndex); if (prefabFish == null) { yield break; } var elapse = 0F; var rotateSpeed = 6.283185F / OneCircleTime;//弧度/s var useTime = OneCircleTime * mRunRound + CenterFishRunoutTime[circleIdx]; var fishCenter = Instantiate(prefabFish); var centerSwimmer = fishCenter.swimmer; fishCenter.ClearAi(); fishCenter.transform.parent = transform; fishCenter.transform.localPosition = new Vector3(Locals_Center[circleIdx].x, Locals_Center[circleIdx].y, Locals_Center[circleIdx].z - 0.1F); fishCenter.transform.localRotation = Quaternion.identity; centerSwimmer.RotateSpd = 6.283185F; while (elapse < useTime) { if (centerSwimmer == null || !fishCenter.Attackable) { break; //暂时用attackable来确定鱼死亡 } //中间鱼转动 centerSwimmer.transform.localRotation = Quaternion.AxisAngle(Vector3.forward, centerSwimmer.RotateSpd); centerSwimmer.RotateSpd -= rotateSpeed * Time.deltaTime; elapse += Time.deltaTime; yield return(0); } if (centerSwimmer != null) { centerSwimmer.Speed = FishRunoutSpeed * 0.7F; centerSwimmer.Go(); } }
public override void Update() { if (frameCount % 100 == 0) { Vector3 axis = Vector3.Normalize(new Vector3(rd.NextDouble() - 0.5, rd.NextDouble() - 0.5, rd.NextDouble() - 0.5)); rot = Quaternion.AxisAngle(axis, Math.PI / 30); } currentRotationMatrix *= Matrix4x4.CreateRotationMatrix(rot); frameCount++; }
// Update is called once per frame void Update( ) { if (_time == 0) { Vector3 ahead = transform.TransformPoint(_foword.position) - transform.TransformPoint(_back.position); Vector3 dir = Vector3.Cross(Vector3.up, ahead).normalized; float angle = Mathf.Acos(Vector3.Dot(Vector3.up, ahead)); Quaternion q = Quaternion.AxisAngle(dir, angle); GameObject arrow = Instantiate(_arrow, transform.position + new Vector3(0, 0.5f, 0), new Quaternion(0, 0, 0, 0)); arrow.AddComponent <Rigidbody>( ); arrow.GetComponent <Rigidbody> ().velocity = ahead.normalized * _arrow_speed; Destroy(arrow, 10.0f); } _time = (_time + 1) % _interval; }
//called by some event //Set the AR camera on real robot and do ResetFrame(), "Unity"TF will fixed with the offset of transform as PoseStamped message (in ROS side script) public void ResetFrameBoth(Transform RefTransform = null) { GameObject go = new GameObject(); if (RefTransform is null) { go.transform.parent = ReferenceTransform; } else { go.transform.parent = RefTransform; } if (BasePlane.IsValid()) { Vector3 normal = BasePlane.rotation * Vector3.down; float b = Vector3.Dot(normal, BasePlane.position); float a = Vector3.Dot(normal, ReferenceTransform.position); go.transform.position = ReferenceTransform.position - (a - b) * normal; //normal direction must be same as base plane. normal = BasePlane.rotation * Vector3.up; Vector3 rotVec = new Vector3( ReferenceTransform.transform.rotation.x, ReferenceTransform.transform.rotation.y, ReferenceTransform.transform.rotation.z); float r = Vector3.Dot(normal, rotVec); go.transform.rotation = Quaternion.AxisAngle(normal, r); } else { go.transform.localPosition = -1 * ResetOffset; go.transform.rotation = ReferenceTransform.rotation; } Matrix4x4 v = TargetTransform.localToWorldMatrix * go.transform.worldToLocalMatrix; Vector3 p = v.GetColumn(3); message.header.Update(); message.pose.position = GetGeometryPoint(p.Unity2Ros()); message.pose.orientation = GetGeometryQuaternion(v.rotation.Unity2Ros()); Publish(message); ToastUtil.Toast(this, "Reset Frame."); Debug.Log("Reset FrameBoth"); }
public IEnumerator updateFacing() { Vector2 previousPosition = transform.position; yield return(new WaitForSeconds(0)); while (true) { Vector2 velocityNow = ((Vector2)transform.position - previousPosition) / Time.deltaTime; if (velocityNow != Vector2.zero) { velocityNow.Normalize(); facing = Mathf.Atan2(velocityNow.x, velocityNow.y); transform.GetChild(0).rotation = Quaternion.AxisAngle(Vector3.forward, facing * -1 - Mathf.PI); } previousPosition = transform.position; yield return(new WaitForSeconds(0.1f)); } }
// Update is called once per frame void Update() { float height = Mathf.Sin(Time.time * Mathf.PI * boinzSpeed) * boinzHeight; if (Mathf.Sign(height) != Mathf.Sign(lastBoinzHeight)) { if (hapticLeft) { VRTK_DeviceFinder.GetControllerLeftHand().GetComponent <VRTK_ControllerActions>().TriggerHapticPulse((ushort)(hapticStrength * 3999)); } if (hapticRight) { VRTK_DeviceFinder.GetControllerRightHand().GetComponent <VRTK_ControllerActions>().TriggerHapticPulse((ushort)(hapticStrength * 3999)); } } lastBoinzHeight = height; transform.localPosition = startOffset + new Vector3(0, Mathf.Abs(height), 0); transform.localRotation = Quaternion.AxisAngle(Vector3.up, Time.time); }
void Update() { if (_enabled) { // Update Position var right = transform.Right; var up = Vector3.Up; var forward = transform.Forward; var movement = _movementInput.x * right + _movementInput.y * up + _movementInput.z * forward; movement = Time.deltaTime * movementSpeed * movement; transform.Position += movement; // Update Rotation var delta = Time.deltaTime * rotateSpeed; var drag = Input.mousePos - _lastMousePos; transform.Rotation = Quaternion.Euler(0, drag.x * delta, 0) * Quaternion.AxisAngle(right, drag.y * delta) * transform.Rotation; } _lastMousePos = Input.mousePos; }
public void ResetFrameRot(Transform RefTransform = null) { GameObject go = new GameObject(); if (RefTransform is null) { go.transform.parent = ReferenceTransform; } else { go.transform.parent = RefTransform; } go.transform.position = TargetTransform.position; if (BasePlane.IsValid()) { //normal direction must be same as base plane. Vector3 normal = BasePlane.rotation * Vector3.up; Vector3 rotVec = new Vector3( ReferenceTransform.transform.rotation.x, ReferenceTransform.transform.rotation.y, ReferenceTransform.transform.rotation.z); float r = Vector3.Dot(normal, rotVec); go.transform.rotation = Quaternion.AxisAngle(normal, r); } else { go.transform.localRotation = Quaternion.identity; } Matrix4x4 v = TargetTransform.localToWorldMatrix * go.transform.worldToLocalMatrix; Destroy(go); Vector3 p = v.GetColumn(3); message.header.Update(); message.pose.position = GetGeometryPoint(p.Unity2Ros()); message.pose.orientation = GetGeometryQuaternion(v.rotation.Unity2Ros()); Publish(message); }
// Called each frame void Update() { // Rotate around Y Axis transform.Rotation *= Quaternion.AxisAngle(Vector3.Up, Time.deltaTime * angularSpeed); }
// Update is called once per frame void Update() { float a = Mathf.Cos(freq * Time.time) * mag; transform.localRotation = Quaternion.AxisAngle(axis, a); }
Bool IEnvironment.match(Vector3[] Rays, out MarkerIndex[] Markers, out Pose PositionofUp) { const float projectionsMatchTolerance = 0.06f; Markers = Enumerable.Repeat(MarkerIndex.Unknown, Rays.Length).ToArray(); PositionofUp = new Pose(); if (Rays.Length != 3) { return(false); } var projecties = projectRaysOnFloor(Rays); var M = new float[6, 4]; var b = new float[6]; for (int i = 0; i < 3; ++i) { M[2 * i, 0] = projecties[i].x; M[2 * i, 1] = -projecties[i].y; M[2 * i, 2] = 1; M[2 * i + 1, 0] = projecties[i].y; M[2 * i + 1, 1] = projecties[i].x; M[2 * i + 1, 3] = 1; b[2 * i] = _markers[i].x; b[2 * i + 1] = _markers[i].y; } var invM = M.transpose().multiply(M).inverse().multiply(M.transpose()); int BestMatch = -1; float bestError = float.MaxValue; var bestTransform2d = new ConvertTo2D(); for (int idPermutation = 0; idPermutation < 6; ++idPermutation) { var curB = new float[6]; for (int i = 0; i < 4; ++i) { int j = permutations[idPermutation][i]; curB[2 * i] = b[2 * j]; curB[2 * i + 1] = b[2 * j + 1]; } var transformParams = invM.multiply(curB); var transform2d = new ConvertTo2D(transformParams[0], transformParams[1], transformParams[2], transformParams[3]); float error = 0; for (int idMarker = 0; idMarker < 3; ++idMarker) { error += (transform2d.apply(projecties[idMarker]) - _markers[permutations[idPermutation][idMarker]]).sqrMagnitude; } if (error < bestError) { bestError = error; BestMatch = idPermutation; bestTransform2d = transform2d; } } if (bestError > projectionsMatchTolerance) { return(false); } for (int i = 0; i < Rays.Length; ++i) { Markers[i].value = (uint)permutations[BestMatch][i]; } var position = new Vector3(bestTransform2d.Translate.x, bestTransform2d.Scale, bestTransform2d.Translate.y); var rotation = Quaternion.AxisAngle(Vector3.up, -bestTransform2d.Angle); PositionofUp = new Pose(position, rotation); lock (_visobject) { _matchViz = new MatchVisualization( Rays.ToList(), PositionofUp, projecties.Select(p => bestTransform2d.apply(p)).ToList(), Markers); } return(true); }
// Update is called once per frame void Update() { transform.localPosition = startOffset + new Vector3(0, Mathf.Abs(Mathf.Sin(Time.time * Mathf.PI * boinzSpeed)) * boinzHeight, 0); transform.localRotation = Quaternion.AxisAngle(Vector3.up, Time.time); }
void Update() { overheatBulletLimit = ButtonController.weaponType2 == 1 ? 10 : 5; if ((Input.GetKeyDown(KeyCode.Keypad4) || (Input.GetKeyDown(KeyCode.LeftBracket))) && !isKnockedDown && bulletNum < overheatBulletLimit) { //Debug.Log("1: " + ButtonController.weaponType1 + ", 2: " + ButtonController.weaponType2); if (ButtonController.weaponType2 != 2) { Instantiate(bullet, rb.transform.position + moveDirection.normalized / 1.5f, Quaternion.identity); onMachineGun.Invoke(); } else { Instantiate(bullet, rb.transform.position + moveDirection.normalized / 1.5f, Quaternion.identity); Instantiate(bullet, rb.transform.position + moveDirection.normalized / 1.5f, Quaternion.AxisAngle(new Vector3(0, 0, 1), 0.1f)); Instantiate(bullet, rb.transform.position + moveDirection.normalized / 1.5f, Quaternion.AxisAngle(new Vector3(0, 0, 1), -0.1f)); onShotGun.Invoke(); } bulletNum++; } if ((Input.GetKeyDown(KeyCode.Keypad4) || (Input.GetKeyDown(KeyCode.LeftBracket))) && !isKnockedDown && bulletNum == overheatBulletLimit) { bulletNum++; overheat.gameObject.SetActive(true); Invoke("ShotReady", 3); } distanceToPlayer1 = (rb.transform.position - PlayerController.instance.transform.position).magnitude; damage = distanceToPlayer1 <= 5 ? powerupDamage : damage; // Power up when two player are close to each other bullet.GetComponent <SpriteRenderer>().color = distanceToPlayer1 <= 5 ? new Color(255, 0, 0) : new Color(255, 255, 255); if (distanceToPlayer1 <= 1.5f && (Input.GetKeyDown(KeyCode.Keypad5) || (Input.GetKeyDown(KeyCode.RightBracket))) && PlayerController.instance.isKnockedDown) { timer += Time.deltaTime; rescueTime.gameObject.SetActive(true); rescueTime.text = (3 - (int)timer) + "sec"; //Debug.Log("Timer: " + timer + ", distance: " + distanceToPlayer1); if (timer >= 3) { onRescuedPlayer1.Invoke(); timer = 0; rescueTime.gameObject.SetActive(false); } } //if (Input.GetKeyUp(KeyCode.Keypad5) || distanceToPlayer1 > 1.5f) // need to hold the key until another player is recovered?? //{ // timer = 0; //} }
// Use this for initialization void Start() { Points = new GameObject[numPoints + 2]; Points[0] = Base; Vector3 dif = Tip.transform.position - Base.transform.position; float length = dif.magnitude; length -= baseSize; length -= tipSize; dif.Normalize(); Rigidbody rb; SpringJoint sj; PlayRandomTouch pt; List <AudioClip> Clips = new List <AudioClip>(); Clips.Add(Resources.Load("Audio/hydra/ArmStroke1") as AudioClip); Clips.Add(Resources.Load("Audio/hydra/ArmStroke2") as AudioClip); Clips.Add(Resources.Load("Audio/hydra/ArmStroke3") as AudioClip); for (int i = 0; i < numPoints; i++) { GameObject capsule = GameObject.CreatePrimitive(PrimitiveType.Capsule); capsule.transform.localScale = new Vector3(pointSize, length / numPoints, pointSize); capsule.transform.position = Base.transform.position + baseSize * dif + length * dif * (((float)i + 0.5f) / (float)numPoints); Vector3 a = Vector3.forward; a = Quaternion.AxisAngle(Vector3.forward, 90) * dif; capsule.transform.rotation = Quaternion.FromToRotation(Vector3.up, dif); capsule.GetComponent <Renderer>().enabled = false; pt = capsule.AddComponent <PlayRandomTouch>(); pt.pitch = 1.0f * Mathf.Floor(3.0f * (float)i / (float)numPoints); pt.time = Random.Range(0, 10); pt.Clips = Clips; pt.volume = 0.6f; rb = capsule.AddComponent <Rigidbody>(); rb.drag = 1; rb.angularDrag = .5f; sj = capsule.AddComponent <SpringJoint>(); sj.spring = 100; sj.damper = 1; sj.anchor = new Vector3(0, -.5f, 0); if (i == 0) { sj.connectedBody = Base.GetComponent <Rigidbody>(); } else { sj.connectedBody = Points[i].GetComponent <Rigidbody>(); } Points[i + 1] = capsule; } sj = Tip.AddComponent <SpringJoint>(); sj.connectedBody = Points[numPoints].GetComponent <Rigidbody>(); sj.spring = 100; sj.damper = 3; sj.anchor = new Vector3(0, -.5f, 0); Points[numPoints + 1] = Tip; addTube(); }
public void UpdateRotation(float angle) { transform.localRotation = Quaternion.AxisAngle(Vector3.up, angle); }
Vector3 rotatedAbout(Vector3 v, Vector3 axis, float angle) { return(Quaternion.AxisAngle(axis, angle) * v); }
public override void Show() { Vector2 from = transform.position; Vector2 to = thisWeapon.target.transform.position; Vector2 direction = from - to; float radAngle = Mathf.Atan2(direction.y, direction.x); GameObject beam = Instantiate((GameObject)Resources.Load("beam"), (transform.position + thisWeapon.target.transform.position) / 2, Quaternion.AxisAngle(Vector3.forward, radAngle)); beam.GetComponent <SpriteRenderer>().size = new Vector2(direction.magnitude, 0.3f); }
protected virtual void OnSceneGUI() { Thruster t = target as Thruster; Handles.ScaleValueHandle(1.0f, t.transform.position, Quaternion.AxisAngle(t.thrustDir, 0), size, Handles.ArrowHandleCap, snap); }
Bool IEnvironment.match(Vector3[] raysUpSpace, out MarkerIndex[] markersIndices, out Pose poseOfUpSpace) { const float projectionsMatchTolerance = 0.05f; markersIndices = Enumerable.Repeat(MarkerIndex.Unknown, raysUpSpace.Length).ToArray(); poseOfUpSpace = new Pose(); // For the sake of simplicity, we won't deal with cases when more than three rays is visible. if (raysUpSpace.Length != 3) { return(false); } // rays projection on X-Y plane of "up space" at height of 1 meter var projections = projectRaysOnFloor(raysUpSpace); // We have to find transform that match rays projections to markers in world space in form f(x) = SR*x + t, where // SR - scale and rotation matrix: s*{{cos(r), -sin(r)}, {sin(r), cos(r)}}; s - scale coefficient, r - rotation angle, // t - translation vector. // Lets solve system of linear equations {SR*ai + t = bi} for SR and t // System can be rewritten as M*x = b // / a0.x -a0.y 1 0 \ / s*cos(r) \ / b0.x \ // | a0.y a0.x 0 1 | * | s*sin(r) | = | b0.y | // | ... | | t.x | | ... | // \ ... / \ t.y / \ ... / var M = new float[6, 4]; var b = new float[6]; for (int i = 0; i < 3; ++i) { M[2 * i, 0] = projections[i].x; M[2 * i, 1] = -projections[i].y; M[2 * i, 2] = 1; M[2 * i + 1, 0] = projections[i].y; M[2 * i + 1, 1] = projections[i].x; M[2 * i + 1, 3] = 1; b[2 * i] = _markers[i].x; b[2 * i + 1] = _markers[i].y; } // left inverse var invM = M.transpose().multiply(M).inverse().multiply(M.transpose()); // Iterate over all possible permutations and pick the best one if its good enough. int bestPermutationId = -1; float bestError = float.MaxValue; var bestTransform2d = new Transform2d(); for (int idPermutation = 0; idPermutation < 6; ++idPermutation) { // make vector of constant terms for current markers order var curB = new float[6]; for (int i = 0; i < 3; ++i) { int j = permutations[idPermutation][i]; curB[2 * i] = b[2 * j]; curB[2 * i + 1] = b[2 * j + 1]; } var transformParams = invM.multiply(curB); // {scale*cos(r), scale*sin(r), t.x, t.y} var transform2d = new Transform2d(transformParams[0], transformParams[1], transformParams[2], transformParams[3]); float error = 0; // sum of square distances between corresponding markers and transformed rays projections for (int idMarker = 0; idMarker < 3; ++idMarker) { error += (transform2d.apply(projections[idMarker]) - _markers[permutations[idPermutation][idMarker]]).sqrMagnitude; } if (error < bestError) { bestError = error; bestPermutationId = idPermutation; bestTransform2d = transform2d; } } if (bestError > projectionsMatchTolerance) { return(false); } for (int i = 0; i < raysUpSpace.Length; ++i) { markersIndices[i].value = (uint)permutations[bestPermutationId][i]; } var position = new Vector3(bestTransform2d.Translate.x, bestTransform2d.Scale, bestTransform2d.Translate.y); var rotation = Quaternion.AxisAngle(Vector3.up, -bestTransform2d.Angle); poseOfUpSpace = new Pose(position, rotation); lock (_visualizationLocker) { _matchVisualization = new MatchVisualization( raysUpSpace.ToList(), poseOfUpSpace, projections.Select(p => bestTransform2d.apply(p)).ToList(), markersIndices); } return(true); }