public override void MoveToDiscretePosition(Draggable draggable) { Vector3 draggableDir = (draggable.transform.position - transform.position).normalized; float draggableDot = Vector3.Dot(draggableDir, hingeVector); draggableDir = draggableDir - (hingeVector * draggableDot); Vector3[] Sides = new Vector3[6]; Sides[0] = Vector3.left; Sides[1] = Vector3.right; Sides[2] = Vector3.down; Sides[3] = Vector3.up; Sides[4] = Vector3.back; Sides[5] = Vector3.forward; float[] Angles = new float[6]; int smallestAngleIndex = 0; for (int i = 0; i < 6; i++) { Angles[i] = Vector3.Angle(draggableDir, Sides[i]); if (i == 1) { if (Angles[i] < Angles[i - 1]) { smallestAngleIndex = i; } else { smallestAngleIndex = i - 1; } } if (i > 1) { if (Angles[i] < Angles[smallestAngleIndex]) { smallestAngleIndex = i; } } } MovePosition(transform.position + Sides[smallestAngleIndex], draggable); }
public override void MovePosition(Vector3 position, Draggable draggable) { Vector3 fromPosition = (draggable.transform.position - transform.position).normalized; Vector3 toPosition = (position - transform.position).normalized; float fromHingeDot = Vector3.Dot(fromPosition, hingeVector); //we calculate dot product between hingeVector and fromPosition and... fromPosition = fromPosition - (hingeVector * fromHingeDot); //...subtract it from the from Position, so that fromPosition is a perpendicular rotation to hinge float toHingeDot = Vector3.Dot(toPosition, hingeVector); // same thing as above two lines toPosition = toPosition - (hingeVector * toHingeDot); Debug.DrawRay(transform.position, fromPosition, Color.red); Debug.DrawRay(transform.position, toPosition, Color.cyan); float fromAngle = Vector3.SignedAngle(fromPosition, perpendicularVector, -hingeVector); float toAngle = Vector3.SignedAngle(toPosition, perpendicularVector, -hingeVector); float Amt = motorPID.SeekAngle(toAngle, fromAngle); rigidbody.AddTorque(hingeVector * Amt, ForceMode.VelocityChange); }
/* * Automatically snaps to the closest position or rotation * */ public abstract void MoveToDiscretePosition(Draggable draggable);
/* * Run the machine to move it in the position * */ public abstract void MovePosition(Vector3 position, Draggable draggable);