private void AddBeforePosition (float p, float[] temp, int i, float[] output, int o) { float x1 = temp[i], y1 = temp[i + 1], dx = temp[i + 2] - x1, dy = temp[i + 3] - y1, r = MathUtils.Atan2(dy, dx); output[o] = x1 + p * MathUtils.Cos(r); output[o + 1] = y1 + p * MathUtils.Sin(r); output[o + 2] = r; }