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