diff --git a/.run/Run Simulator.run.xml b/.run/Run Simulator.run.xml index df0d196d..2e975946 100644 --- a/.run/Run Simulator.run.xml +++ b/.run/Run Simulator.run.xml @@ -1,23 +1,24 @@ - - - - - - - true - true - false - - + + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/Common/src/main/java/android/opengl/Matrix.java b/Common/src/main/java/android/opengl/Matrix.java new file mode 100644 index 00000000..709339ef --- /dev/null +++ b/Common/src/main/java/android/opengl/Matrix.java @@ -0,0 +1,853 @@ +/* + * Copyright (C) 2007 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package android.opengl; + +import java.util.Objects; + +/** + * Matrix math utilities. These methods operate on OpenGL ES format + * matrices and vectors stored in float arrays. + *

+ * Matrices are 4 x 4 column-vector matrices stored in column-major + * order: + * 

+ *  m[offset +  0] m[offset +  4] m[offset +  8] m[offset + 12]
+ *  m[offset +  1] m[offset +  5] m[offset +  9] m[offset + 13]
+ *  m[offset +  2] m[offset +  6] m[offset + 10] m[offset + 14]
+ *  m[offset +  3] m[offset +  7] m[offset + 11] m[offset + 15]
+ * + * Vectors are 4 x 1 column vectors stored in order: + * 
+ * v[offset + 0]
+ * v[offset + 1]
+ * v[offset + 2]
+ * v[offset + 3]
+ */ +public class Matrix { + + /** Temporary memory for operations that need temporary matrix data. */ + private final static float[] sTemp = new float[32]; + + /** + * @deprecated All methods are static, do not instantiate this class. + */ + @Deprecated + public Matrix() {} + + // Helper function to calculate the index in the 1D array representing a 4x4 matrix. + private static int I(int i, int j) { + return j + 4 * i; + } + + /** + * Multiplies two 4x4 matrices together and stores the result in a third 4x4 + * matrix. In matrix notation: result = lhs x rhs. Due to the way + * matrix multiplication works, the result matrix will have the same + * effect as first multiplying by the rhs matrix, then multiplying by + * the lhs matrix. This is the opposite of what you might expect. + *

+ * The same float array may be passed for result, lhs, and/or rhs. However, + * the result element values are undefined if the result elements overlap + * either the lhs or rhs elements. + * + * @param result The float array that holds the result. + * @param resultOffset The offset into the result array where the result is + * stored. + * @param lhs The float array that holds the left-hand-side matrix. + * @param lhsOffset The offset into the lhs array where the lhs is stored + * @param rhs The float array that holds the right-hand-side matrix. + * @param rhsOffset The offset into the rhs array where the rhs is stored. + * + * @throws IllegalArgumentException if result, lhs, or rhs are null, or if + * resultOffset + 16 > result.length or lhsOffset + 16 > lhs.length or + * rhsOffset + 16 > rhs.length. + */ + // Java implementation of multiplyMM with offsets + public static void multiplyMM(float[] result, int resultOffset, + float[] lhs, int lhsOffset, float[] rhs, int rhsOffset) { + if(result == null) { + throw new IllegalArgumentException("result must not be null"); + } + if(lhs == null) { + throw new IllegalArgumentException("lhs must not be null"); + } + if(rhs == null) { + throw new IllegalArgumentException("rhs must not be null"); + } + + if (resultOffset + 16 > result.length) { + throw new IllegalArgumentException("resultOffset + 16 > result.length"); + } + + if (lhsOffset + 16 > lhs.length) { + throw new IllegalArgumentException("lhsOffset + 16 > lhs.length"); + } + + if (rhsOffset + 16 > rhs.length) { + throw new IllegalArgumentException("rhsOffset + 16 > rhs.length"); + } + + // Multiply the two 4x4 matrices with the given offsets. + for (int i = 0; i < 4; i++) { + final float rhs_i0 = rhs[rhsOffset + I(i, 0)]; + float ri0 = lhs[lhsOffset + I(0, 0)] * rhs_i0; + float ri1 = lhs[lhsOffset + I(0, 1)] * rhs_i0; + float ri2 = lhs[lhsOffset + I(0, 2)] * rhs_i0; + float ri3 = lhs[lhsOffset + I(0, 3)] * rhs_i0; + + for (int j = 1; j < 4; j++) { + final float rhs_ij = rhs[rhsOffset + I(i, j)]; + ri0 += lhs[lhsOffset + I(j, 0)] * rhs_ij; + ri1 += lhs[lhsOffset + I(j, 1)] * rhs_ij; + ri2 += lhs[lhsOffset + I(j, 2)] * rhs_ij; + ri3 += lhs[lhsOffset + I(j, 3)] * rhs_ij; + } + + result[resultOffset + I(i, 0)] = ri0; + result[resultOffset + I(i, 1)] = ri1; + result[resultOffset + I(i, 2)] = ri2; + result[resultOffset + I(i, 3)] = ri3; + } + } + + private static void mx4transform(float[] lhsMat, int lhsMatOffset, + float[] rhsVec, int rhsVecOffset, + float[] resultVec, int resultVecOffset) { + resultVec[resultVecOffset + 0] = lhsMat[lhsMatOffset + 0 + 4 * 0] * rhsVec[rhsVecOffset + 0] + + lhsMat[lhsMatOffset + 0 + 4 * 1] * rhsVec[rhsVecOffset + 1] + + lhsMat[lhsMatOffset + 0 + 4 * 2] * rhsVec[rhsVecOffset + 2] + + lhsMat[lhsMatOffset + 0 + 4 * 3] * rhsVec[rhsVecOffset + 3]; + + resultVec[resultVecOffset + 1] = lhsMat[lhsMatOffset + 1 + 4 * 0] * rhsVec[rhsVecOffset + 0] + + lhsMat[lhsMatOffset + 1 + 4 * 1] * rhsVec[rhsVecOffset + 1] + + lhsMat[lhsMatOffset + 1 + 4 * 2] * rhsVec[rhsVecOffset + 2] + + lhsMat[lhsMatOffset + 1 + 4 * 3] * rhsVec[rhsVecOffset + 3]; + + resultVec[resultVecOffset + 2] = lhsMat[lhsMatOffset + 2 + 4 * 0] * rhsVec[rhsVecOffset + 0] + + lhsMat[lhsMatOffset + 2 + 4 * 1] * rhsVec[rhsVecOffset + 1] + + lhsMat[lhsMatOffset + 2 + 4 * 2] * rhsVec[rhsVecOffset + 2] + + lhsMat[lhsMatOffset + 2 + 4 * 3] * rhsVec[rhsVecOffset + 3]; + + resultVec[resultVecOffset + 3] = lhsMat[lhsMatOffset + 3 + 4 * 0] * rhsVec[rhsVecOffset + 0] + + lhsMat[lhsMatOffset + 3 + 4 * 1] * rhsVec[rhsVecOffset + 1] + + lhsMat[lhsMatOffset + 3 + 4 * 2] * rhsVec[rhsVecOffset + 2] + + lhsMat[lhsMatOffset + 3 + 4 * 3] * rhsVec[rhsVecOffset + 3]; + } + + /** + * Multiplies a 4 element vector by a 4x4 matrix and stores the result in a + * 4-element column vector. In matrix notation: result = lhs x rhs + *

+ * The same float array may be passed for resultVec, lhsMat, and/or rhsVec. + * However, the resultVec element values are undefined if the resultVec + * elements overlap either the lhsMat or rhsVec elements. + * + * @param resultVec The float array that holds the result vector. + * @param resultVecOffset The offset into the result array where the result + * vector is stored. + * @param lhsMat The float array that holds the left-hand-side matrix. + * @param lhsMatOffset The offset into the lhs array where the lhs is stored + * @param rhsVec The float array that holds the right-hand-side vector. + * @param rhsVecOffset The offset into the rhs vector where the rhs vector + * is stored. + * + * @throws IllegalArgumentException if resultVec, lhsMat, + * or rhsVec are null, or if resultVecOffset + 4 > resultVec.length + * or lhsMatOffset + 16 > lhsMat.length or + * rhsVecOffset + 4 > rhsVec.length. + */ + public static void multiplyMV(float[] resultVec, + int resultVecOffset, float[] lhsMat, int lhsMatOffset, + float[] rhsVec, int rhsVecOffset) { + if(resultVec == null) { + throw new IllegalArgumentException("resultVec must not be null"); + } + + if(lhsMat == null) { + throw new IllegalArgumentException("lhsMat must not be null"); + } + + if(rhsVec == null) { + throw new IllegalArgumentException("rhsVec must not be null"); + } + + if(resultVecOffset + 4 > resultVec.length) { + throw new IllegalArgumentException("resultVecOffset + 4 > resultVec.length"); + } + if(lhsMatOffset + 16 > lhsMat.length) { + throw new IllegalArgumentException("lhsMatOffset + 16 > lhsMat.length"); + } + if(rhsVecOffset + 4 > rhsVec.length) { + throw new IllegalArgumentException("rhsVecOffset + 4 > rhsVec.length"); + } + + mx4transform(lhsMat, lhsMatOffset, rhsVec, rhsVecOffset, resultVec, resultVecOffset); + } + + /** + * Transposes a 4 x 4 matrix. + *

+ * mTrans and m must not overlap. + * + * @param mTrans the array that holds the output transposed matrix + * @param mTransOffset an offset into mTrans where the transposed matrix is + * stored. + * @param m the input array + * @param mOffset an offset into m where the input matrix is stored. + */ + public static void transposeM(float[] mTrans, int mTransOffset, float[] m, + int mOffset) { + for (int i = 0; i < 4; i++) { + int mBase = i * 4 + mOffset; + mTrans[i + mTransOffset] = m[mBase]; + mTrans[i + 4 + mTransOffset] = m[mBase + 1]; + mTrans[i + 8 + mTransOffset] = m[mBase + 2]; + mTrans[i + 12 + mTransOffset] = m[mBase + 3]; + } + } + + /** + * Inverts a 4 x 4 matrix. + *

+ * mInv and m must not overlap. + * + * @param mInv the array that holds the output inverted matrix + * @param mInvOffset an offset into mInv where the inverted matrix is + * stored. + * @param m the input array + * @param mOffset an offset into m where the input matrix is stored. + * @return true if the matrix could be inverted, false if it could not. + */ + public static boolean invertM(float[] mInv, int mInvOffset, float[] m, + int mOffset) { + // Invert a 4 x 4 matrix using Cramer's Rule + + // transpose matrix + final float src0 = m[mOffset + 0]; + final float src4 = m[mOffset + 1]; + final float src8 = m[mOffset + 2]; + final float src12 = m[mOffset + 3]; + + final float src1 = m[mOffset + 4]; + final float src5 = m[mOffset + 5]; + final float src9 = m[mOffset + 6]; + final float src13 = m[mOffset + 7]; + + final float src2 = m[mOffset + 8]; + final float src6 = m[mOffset + 9]; + final float src10 = m[mOffset + 10]; + final float src14 = m[mOffset + 11]; + + final float src3 = m[mOffset + 12]; + final float src7 = m[mOffset + 13]; + final float src11 = m[mOffset + 14]; + final float src15 = m[mOffset + 15]; + + // calculate pairs for first 8 elements (cofactors) + final float atmp0 = src10 * src15; + final float atmp1 = src11 * src14; + final float atmp2 = src9 * src15; + final float atmp3 = src11 * src13; + final float atmp4 = src9 * src14; + final float atmp5 = src10 * src13; + final float atmp6 = src8 * src15; + final float atmp7 = src11 * src12; + final float atmp8 = src8 * src14; + final float atmp9 = src10 * src12; + final float atmp10 = src8 * src13; + final float atmp11 = src9 * src12; + + // calculate first 8 elements (cofactors) + final float dst0 = (atmp0 * src5 + atmp3 * src6 + atmp4 * src7) + - (atmp1 * src5 + atmp2 * src6 + atmp5 * src7); + final float dst1 = (atmp1 * src4 + atmp6 * src6 + atmp9 * src7) + - (atmp0 * src4 + atmp7 * src6 + atmp8 * src7); + final float dst2 = (atmp2 * src4 + atmp7 * src5 + atmp10 * src7) + - (atmp3 * src4 + atmp6 * src5 + atmp11 * src7); + final float dst3 = (atmp5 * src4 + atmp8 * src5 + atmp11 * src6) + - (atmp4 * src4 + atmp9 * src5 + atmp10 * src6); + final float dst4 = (atmp1 * src1 + atmp2 * src2 + atmp5 * src3) + - (atmp0 * src1 + atmp3 * src2 + atmp4 * src3); + final float dst5 = (atmp0 * src0 + atmp7 * src2 + atmp8 * src3) + - (atmp1 * src0 + atmp6 * src2 + atmp9 * src3); + final float dst6 = (atmp3 * src0 + atmp6 * src1 + atmp11 * src3) + - (atmp2 * src0 + atmp7 * src1 + atmp10 * src3); + final float dst7 = (atmp4 * src0 + atmp9 * src1 + atmp10 * src2) + - (atmp5 * src0 + atmp8 * src1 + atmp11 * src2); + + // calculate pairs for second 8 elements (cofactors) + final float btmp0 = src2 * src7; + final float btmp1 = src3 * src6; + final float btmp2 = src1 * src7; + final float btmp3 = src3 * src5; + final float btmp4 = src1 * src6; + final float btmp5 = src2 * src5; + final float btmp6 = src0 * src7; + final float btmp7 = src3 * src4; + final float btmp8 = src0 * src6; + final float btmp9 = src2 * src4; + final float btmp10 = src0 * src5; + final float btmp11 = src1 * src4; + + // calculate second 8 elements (cofactors) + final float dst8 = (btmp0 * src13 + btmp3 * src14 + btmp4 * src15) + - (btmp1 * src13 + btmp2 * src14 + btmp5 * src15); + final float dst9 = (btmp1 * src12 + btmp6 * src14 + btmp9 * src15) + - (btmp0 * src12 + btmp7 * src14 + btmp8 * src15); + final float dst10 = (btmp2 * src12 + btmp7 * src13 + btmp10 * src15) + - (btmp3 * src12 + btmp6 * src13 + btmp11 * src15); + final float dst11 = (btmp5 * src12 + btmp8 * src13 + btmp11 * src14) + - (btmp4 * src12 + btmp9 * src13 + btmp10 * src14); + final float dst12 = (btmp2 * src10 + btmp5 * src11 + btmp1 * src9 ) + - (btmp4 * src11 + btmp0 * src9 + btmp3 * src10); + final float dst13 = (btmp8 * src11 + btmp0 * src8 + btmp7 * src10) + - (btmp6 * src10 + btmp9 * src11 + btmp1 * src8 ); + final float dst14 = (btmp6 * src9 + btmp11 * src11 + btmp3 * src8 ) + - (btmp10 * src11 + btmp2 * src8 + btmp7 * src9 ); + final float dst15 = (btmp10 * src10 + btmp4 * src8 + btmp9 * src9 ) + - (btmp8 * src9 + btmp11 * src10 + btmp5 * src8 ); + + // calculate determinant + final float det = + src0 * dst0 + src1 * dst1 + src2 * dst2 + src3 * dst3; + + if (det == 0.0f) { + return false; + } + + // calculate matrix inverse + final float invdet = 1.0f / det; + mInv[ mInvOffset] = dst0 * invdet; + mInv[ 1 + mInvOffset] = dst1 * invdet; + mInv[ 2 + mInvOffset] = dst2 * invdet; + mInv[ 3 + mInvOffset] = dst3 * invdet; + + mInv[ 4 + mInvOffset] = dst4 * invdet; + mInv[ 5 + mInvOffset] = dst5 * invdet; + mInv[ 6 + mInvOffset] = dst6 * invdet; + mInv[ 7 + mInvOffset] = dst7 * invdet; + + mInv[ 8 + mInvOffset] = dst8 * invdet; + mInv[ 9 + mInvOffset] = dst9 * invdet; + mInv[10 + mInvOffset] = dst10 * invdet; + mInv[11 + mInvOffset] = dst11 * invdet; + + mInv[12 + mInvOffset] = dst12 * invdet; + mInv[13 + mInvOffset] = dst13 * invdet; + mInv[14 + mInvOffset] = dst14 * invdet; + mInv[15 + mInvOffset] = dst15 * invdet; + + return true; + } + + /** + * Computes an orthographic projection matrix. + * + * @param m returns the result + * @param mOffset + * @param left + * @param right + * @param bottom + * @param top + * @param near + * @param far + */ + public static void orthoM(float[] m, int mOffset, + float left, float right, float bottom, float top, + float near, float far) { + if (left == right) { + throw new IllegalArgumentException("left == right"); + } + if (bottom == top) { + throw new IllegalArgumentException("bottom == top"); + } + if (near == far) { + throw new IllegalArgumentException("near == far"); + } + + final float r_width = 1.0f / (right - left); + final float r_height = 1.0f / (top - bottom); + final float r_depth = 1.0f / (far - near); + final float x = 2.0f * (r_width); + final float y = 2.0f * (r_height); + final float z = -2.0f * (r_depth); + final float tx = -(right + left) * r_width; + final float ty = -(top + bottom) * r_height; + final float tz = -(far + near) * r_depth; + m[mOffset + 0] = x; + m[mOffset + 5] = y; + m[mOffset +10] = z; + m[mOffset +12] = tx; + m[mOffset +13] = ty; + m[mOffset +14] = tz; + m[mOffset +15] = 1.0f; + m[mOffset + 1] = 0.0f; + m[mOffset + 2] = 0.0f; + m[mOffset + 3] = 0.0f; + m[mOffset + 4] = 0.0f; + m[mOffset + 6] = 0.0f; + m[mOffset + 7] = 0.0f; + m[mOffset + 8] = 0.0f; + m[mOffset + 9] = 0.0f; + m[mOffset + 11] = 0.0f; + } + + + /** + * Defines a projection matrix in terms of six clip planes. + * + * @param m the float array that holds the output perspective matrix + * @param offset the offset into float array m where the perspective + * matrix data is written + * @param left + * @param right + * @param bottom + * @param top + * @param near + * @param far + */ + public static void frustumM(float[] m, int offset, + float left, float right, float bottom, float top, + float near, float far) { + if (left == right) { + throw new IllegalArgumentException("left == right"); + } + if (top == bottom) { + throw new IllegalArgumentException("top == bottom"); + } + if (near == far) { + throw new IllegalArgumentException("near == far"); + } + if (near <= 0.0f) { + throw new IllegalArgumentException("near <= 0.0f"); + } + if (far <= 0.0f) { + throw new IllegalArgumentException("far <= 0.0f"); + } + final float r_width = 1.0f / (right - left); + final float r_height = 1.0f / (top - bottom); + final float r_depth = 1.0f / (near - far); + final float x = 2.0f * (near * r_width); + final float y = 2.0f * (near * r_height); + final float A = (right + left) * r_width; + final float B = (top + bottom) * r_height; + final float C = (far + near) * r_depth; + final float D = 2.0f * (far * near * r_depth); + m[offset + 0] = x; + m[offset + 5] = y; + m[offset + 8] = A; + m[offset + 9] = B; + m[offset + 10] = C; + m[offset + 14] = D; + m[offset + 11] = -1.0f; + m[offset + 1] = 0.0f; + m[offset + 2] = 0.0f; + m[offset + 3] = 0.0f; + m[offset + 4] = 0.0f; + m[offset + 6] = 0.0f; + m[offset + 7] = 0.0f; + m[offset + 12] = 0.0f; + m[offset + 13] = 0.0f; + m[offset + 15] = 0.0f; + } + + /** + * Defines a projection matrix in terms of a field of view angle, an + * aspect ratio, and z clip planes. + * + * @param m the float array that holds the perspective matrix + * @param offset the offset into float array m where the perspective + * matrix data is written + * @param fovy field of view in y direction, in degrees + * @param aspect width to height aspect ratio of the viewport + * @param zNear + * @param zFar + */ + public static void perspectiveM(float[] m, int offset, + float fovy, float aspect, float zNear, float zFar) { + float f = 1.0f / (float) Math.tan(fovy * (Math.PI / 360.0)); + float rangeReciprocal = 1.0f / (zNear - zFar); + + m[offset + 0] = f / aspect; + m[offset + 1] = 0.0f; + m[offset + 2] = 0.0f; + m[offset + 3] = 0.0f; + + m[offset + 4] = 0.0f; + m[offset + 5] = f; + m[offset + 6] = 0.0f; + m[offset + 7] = 0.0f; + + m[offset + 8] = 0.0f; + m[offset + 9] = 0.0f; + m[offset + 10] = (zFar + zNear) * rangeReciprocal; + m[offset + 11] = -1.0f; + + m[offset + 12] = 0.0f; + m[offset + 13] = 0.0f; + m[offset + 14] = 2.0f * zFar * zNear * rangeReciprocal; + m[offset + 15] = 0.0f; + } + + /** + * Computes the length of a vector. + * + * @param x x coordinate of a vector + * @param y y coordinate of a vector + * @param z z coordinate of a vector + * @return the length of a vector + */ + public static float length(float x, float y, float z) { + return (float) Math.sqrt(x * x + y * y + z * z); + } + + /** + * Sets matrix m to the identity matrix. + * + * @param sm returns the result + * @param smOffset index into sm where the result matrix starts + */ + public static void setIdentityM(float[] sm, int smOffset) { + for (int i=0 ; i<16 ; i++) { + sm[smOffset + i] = 0; + } + for(int i = 0; i < 16; i += 5) { + sm[smOffset + i] = 1.0f; + } + } + + /** + * Scales matrix m by x, y, and z, putting the result in sm. + *

+ * m and sm must not overlap. + * + * @param sm returns the result + * @param smOffset index into sm where the result matrix starts + * @param m source matrix + * @param mOffset index into m where the source matrix starts + * @param x scale factor x + * @param y scale factor y + * @param z scale factor z + */ + public static void scaleM(float[] sm, int smOffset, + float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<4 ; i++) { + int smi = smOffset + i; + int mi = mOffset + i; + sm[ smi] = m[ mi] * x; + sm[ 4 + smi] = m[ 4 + mi] * y; + sm[ 8 + smi] = m[ 8 + mi] * z; + sm[12 + smi] = m[12 + mi]; + } + } + + /** + * Scales matrix m in place by sx, sy, and sz. + * + * @param m matrix to scale + * @param mOffset index into m where the matrix starts + * @param x scale factor x + * @param y scale factor y + * @param z scale factor z + */ + public static void scaleM(float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<4 ; i++) { + int mi = mOffset + i; + m[ mi] *= x; + m[ 4 + mi] *= y; + m[ 8 + mi] *= z; + } + } + + /** + * Translates matrix m by x, y, and z, putting the result in tm. + *

+ * m and tm must not overlap. + * + * @param tm returns the result + * @param tmOffset index into sm where the result matrix starts + * @param m source matrix + * @param mOffset index into m where the source matrix starts + * @param x translation factor x + * @param y translation factor y + * @param z translation factor z + */ + public static void translateM(float[] tm, int tmOffset, + float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<12 ; i++) { + tm[tmOffset + i] = m[mOffset + i]; + } + for (int i=0 ; i<4 ; i++) { + int tmi = tmOffset + i; + int mi = mOffset + i; + tm[12 + tmi] = m[mi] * x + m[4 + mi] * y + m[8 + mi] * z + + m[12 + mi]; + } + } + + /** + * Translates matrix m by x, y, and z in place. + * + * @param m matrix + * @param mOffset index into m where the matrix starts + * @param x translation factor x + * @param y translation factor y + * @param z translation factor z + */ + public static void translateM( + float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<4 ; i++) { + int mi = mOffset + i; + m[12 + mi] += m[mi] * x + m[4 + mi] * y + m[8 + mi] * z; + } + } + + /** + * Rotates matrix m by angle a (in degrees) around the axis (x, y, z). + *

+ * m and rm must not overlap. + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param m source matrix + * @param mOffset index into m where the source matrix starts + * @param a angle to rotate in degrees + * @param x X axis component + * @param y Y axis component + * @param z Z axis component + */ + public static void rotateM(float[] rm, int rmOffset, + float[] m, int mOffset, + float a, float x, float y, float z) { + synchronized(sTemp) { + setRotateM(sTemp, 0, a, x, y, z); + multiplyMM(rm, rmOffset, m, mOffset, sTemp, 0); + } + } + + /** + * Rotates matrix m in place by angle a (in degrees) + * around the axis (x, y, z). + * + * @param m source matrix + * @param mOffset index into m where the matrix starts + * @param a angle to rotate in degrees + * @param x X axis component + * @param y Y axis component + * @param z Z axis component + */ + public static void rotateM(float[] m, int mOffset, + float a, float x, float y, float z) { + synchronized(sTemp) { + setRotateM(sTemp, 0, a, x, y, z); + multiplyMM(sTemp, 16, m, mOffset, sTemp, 0); + System.arraycopy(sTemp, 16, m, mOffset, 16); + } + } + + /** + * Creates a matrix for rotation by angle a (in degrees) + * around the axis (x, y, z). + *

+ * An optimized path will be used for rotation about a major axis + * (e.g. x=1.0f y=0.0f z=0.0f). + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param a angle to rotate in degrees + * @param x X axis component + * @param y Y axis component + * @param z Z axis component + */ + public static void setRotateM(float[] rm, int rmOffset, + float a, float x, float y, float z) { + rm[rmOffset + 3] = 0; + rm[rmOffset + 7] = 0; + rm[rmOffset + 11]= 0; + rm[rmOffset + 12]= 0; + rm[rmOffset + 13]= 0; + rm[rmOffset + 14]= 0; + rm[rmOffset + 15]= 1; + a *= (float) (Math.PI / 180.0f); + float s = (float) Math.sin(a); + float c = (float) Math.cos(a); + if (1.0f == x && 0.0f == y && 0.0f == z) { + rm[rmOffset + 5] = c; rm[rmOffset + 10]= c; + rm[rmOffset + 6] = s; rm[rmOffset + 9] = -s; + rm[rmOffset + 1] = 0; rm[rmOffset + 2] = 0; + rm[rmOffset + 4] = 0; rm[rmOffset + 8] = 0; + rm[rmOffset + 0] = 1; + } else if (0.0f == x && 1.0f == y && 0.0f == z) { + rm[rmOffset + 0] = c; rm[rmOffset + 10]= c; + rm[rmOffset + 8] = s; rm[rmOffset + 2] = -s; + rm[rmOffset + 1] = 0; rm[rmOffset + 4] = 0; + rm[rmOffset + 6] = 0; rm[rmOffset + 9] = 0; + rm[rmOffset + 5] = 1; + } else if (0.0f == x && 0.0f == y && 1.0f == z) { + rm[rmOffset + 0] = c; rm[rmOffset + 5] = c; + rm[rmOffset + 1] = s; rm[rmOffset + 4] = -s; + rm[rmOffset + 2] = 0; rm[rmOffset + 6] = 0; + rm[rmOffset + 8] = 0; rm[rmOffset + 9] = 0; + rm[rmOffset + 10]= 1; + } else { + float len = length(x, y, z); + if (1.0f != len) { + float recipLen = 1.0f / len; + x *= recipLen; + y *= recipLen; + z *= recipLen; + } + float nc = 1.0f - c; + float xy = x * y; + float yz = y * z; + float zx = z * x; + float xs = x * s; + float ys = y * s; + float zs = z * s; + rm[rmOffset + 0] = x*x*nc + c; + rm[rmOffset + 4] = xy*nc - zs; + rm[rmOffset + 8] = zx*nc + ys; + rm[rmOffset + 1] = xy*nc + zs; + rm[rmOffset + 5] = y*y*nc + c; + rm[rmOffset + 9] = yz*nc - xs; + rm[rmOffset + 2] = zx*nc - ys; + rm[rmOffset + 6] = yz*nc + xs; + rm[rmOffset + 10] = z*z*nc + c; + } + } + + /** + * Converts Euler angles to a rotation matrix. + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param x angle of rotation, in degrees + * @param y angle of rotation, in degrees + * @param z angle of rotation, in degrees + */ + public static void setRotateEulerM(float[] rm, int rmOffset, + float x, float y, float z) { + x *= (float) (Math.PI / 180.0f); + y *= (float) (Math.PI / 180.0f); + z *= (float) (Math.PI / 180.0f); + float cx = (float) Math.cos(x); + float sx = (float) Math.sin(x); + float cy = (float) Math.cos(y); + float sy = (float) Math.sin(y); + float cz = (float) Math.cos(z); + float sz = (float) Math.sin(z); + float cxsy = cx * sy; + float sxsy = sx * sy; + + rm[rmOffset + 0] = cy * cz; + rm[rmOffset + 1] = -cy * sz; + rm[rmOffset + 2] = sy; + rm[rmOffset + 3] = 0.0f; + + rm[rmOffset + 4] = cxsy * cz + cx * sz; + rm[rmOffset + 5] = -cxsy * sz + cx * cz; + rm[rmOffset + 6] = -sx * cy; + rm[rmOffset + 7] = 0.0f; + + rm[rmOffset + 8] = -sxsy * cz + sx * sz; + rm[rmOffset + 9] = sxsy * sz + sx * cz; + rm[rmOffset + 10] = cx * cy; + rm[rmOffset + 11] = 0.0f; + + rm[rmOffset + 12] = 0.0f; + rm[rmOffset + 13] = 0.0f; + rm[rmOffset + 14] = 0.0f; + rm[rmOffset + 15] = 1.0f; + } + + /** + * Defines a viewing transformation in terms of an eye point, a center of + * view, and an up vector. + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param eyeX eye point X + * @param eyeY eye point Y + * @param eyeZ eye point Z + * @param centerX center of view X + * @param centerY center of view Y + * @param centerZ center of view Z + * @param upX up vector X + * @param upY up vector Y + * @param upZ up vector Z + */ + public static void setLookAtM(float[] rm, int rmOffset, + float eyeX, float eyeY, float eyeZ, + float centerX, float centerY, float centerZ, float upX, float upY, + float upZ) { + + // See the OpenGL GLUT documentation for gluLookAt for a description + // of the algorithm. We implement it in a straightforward way: + + float fx = centerX - eyeX; + float fy = centerY - eyeY; + float fz = centerZ - eyeZ; + + // Normalize f + float rlf = 1.0f / Matrix.length(fx, fy, fz); + fx *= rlf; + fy *= rlf; + fz *= rlf; + + // compute s = f x up (x means "cross product") + float sx = fy * upZ - fz * upY; + float sy = fz * upX - fx * upZ; + float sz = fx * upY - fy * upX; + + // and normalize s + float rls = 1.0f / Matrix.length(sx, sy, sz); + sx *= rls; + sy *= rls; + sz *= rls; + + // compute u = s x f + float ux = sy * fz - sz * fy; + float uy = sz * fx - sx * fz; + float uz = sx * fy - sy * fx; + + rm[rmOffset + 0] = sx; + rm[rmOffset + 1] = ux; + rm[rmOffset + 2] = -fx; + rm[rmOffset + 3] = 0.0f; + + rm[rmOffset + 4] = sy; + rm[rmOffset + 5] = uy; + rm[rmOffset + 6] = -fy; + rm[rmOffset + 7] = 0.0f; + + rm[rmOffset + 8] = sz; + rm[rmOffset + 9] = uz; + rm[rmOffset + 10] = -fz; + rm[rmOffset + 11] = 0.0f; + + rm[rmOffset + 12] = 0.0f; + rm[rmOffset + 13] = 0.0f; + rm[rmOffset + 14] = 0.0f; + rm[rmOffset + 15] = 1.0f; + + translateM(rm, rmOffset, -eyeX, -eyeY, -eyeZ); + } +} \ No newline at end of file diff --git a/Common/src/main/java/com/android/internal/util/ArrayUtils.java b/Common/src/main/java/com/android/internal/util/ArrayUtils.java index 2e30c597..4af45f05 100644 --- a/Common/src/main/java/com/android/internal/util/ArrayUtils.java +++ b/Common/src/main/java/com/android/internal/util/ArrayUtils.java @@ -41,6 +41,35 @@ public class ArrayUtils { public static final File[] EMPTY_FILE = new File[0]; private ArrayUtils() { /* cannot be instantiated */ } + + public static int idealByteArraySize(int need) { + for (int i = 4; i < 32; i++) + if (need <= (1 << i) - 12) + return (1 << i) - 12; + return need; + } + public static int idealBooleanArraySize(int need) { + return idealByteArraySize(need); + } + public static int idealShortArraySize(int need) { + return idealByteArraySize(need * 2) / 2; + } + public static int idealCharArraySize(int need) { + return idealByteArraySize(need * 2) / 2; + } + public static int idealIntArraySize(int need) { + return idealByteArraySize(need * 4) / 4; + } + public static int idealFloatArraySize(int need) { + return idealByteArraySize(need * 4) / 4; + } + public static int idealObjectArraySize(int need) { + return idealByteArraySize(need * 4) / 4; + } + public static int idealLongArraySize(int need) { + return idealByteArraySize(need * 8) / 8; + } + public static byte[] newUnpaddedByteArray(int minLen) { return (byte[])VMRuntime.getRuntime().newUnpaddedArray(byte.class, minLen); } diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Const.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Const.java new file mode 100644 index 00000000..97652a60 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Const.java @@ -0,0 +1,54 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external; + +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * {@link Const} documents a method that promises not to change the internal state + * of the method receiver. Documenting methods in this way helps programmers understand + * which methods examine the object and return results based on that examination but don't + * change the internal object state and which methods, by contrast, perform their function + * but updating or changing internal object state. + * @see NonConst + */ +@Documented +@Target(ElementType.METHOD) +@Retention(RetentionPolicy.CLASS) +public @interface Const +{ +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/NonConst.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/NonConst.java new file mode 100644 index 00000000..36f8dffb --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/NonConst.java @@ -0,0 +1,54 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external; + +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * {@link NonConst} documents a method that performs its function by updating internal + * state of the method receiver. Documenting methods in this way helps programmers understand + * which methods examine the object and return results based on that examination but don't + * change the internal object state and which methods, by contrast, perform their function + * but updating or changing internal object state. + * @see Const + */ +@Documented +@Target(ElementType.METHOD) +@Retention(RetentionPolicy.CLASS) +public @interface NonConst +{ +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Telemetry.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Telemetry.java index d862ac22..b15b12e9 100644 --- a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Telemetry.java +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/Telemetry.java @@ -1,17 +1,23 @@ /* Copyright (c) 2016 Robert Atkinson + All rights reserved. + Redistribution and use in source and binary forms, with or without modification, are permitted (subject to the limitations in the disclaimer below) provided that the following conditions are met: + Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + Neither the name of Robert Atkinson nor the names of his contributors may be used to endorse or promote products derived from this software without specific prior written permission. + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, @@ -26,6 +32,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that */ package org.firstinspires.ftc.robotcore.external; +import androidx.annotation.Nullable; import java.util.Locale; @@ -43,12 +50,18 @@ are permitted (subject to the limitations in the disclaimer below) provided that * telemetry.update(); * * + *

In the 2015/16 season, the call to {@link #update()} was not required; now, however, + * in a LinearOpMode, unless {@link #update()} is called, nothing will appear on the + * driver station screen. In other, loop-based OpModes, {@link #update()} continues to be called + * automatically at the end of OpMode#loop() and OpMode#init_loop(); no call to + * {@link #update()} is required in loop-based OpModes.

* * 
- *     // loop-based opmode
+ *     // loop-based OpMode
  *     telemetry.addData("count", currentCount);
  *     telemetry.addData("elapsedTime", "%.3f", elapsedSeconds);
  *
+ *

By default (but see {@link #setAutoClear(boolean) setAutoClear()}), data is cleared from the * telemetry after each call to {@link #update()}; thus, you need to issue {@link #addData(String, * Object) addData()} for the entire contents of the telemetry screen on each update cycle. @@ -69,6 +82,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that * telemetry.update(); * ... * } + * void anotherPartOfYourCode() { * ... * elapsedItem.setValue("%.3f", elapsedSeconds); @@ -268,6 +282,30 @@ public interface Telemetry */ boolean removeAction(Object token); + //---------------------------------------------------------------------------------------------- + // Text to Speech + //---------------------------------------------------------------------------------------------- + + /** + * Directs the Driver Station device to speak the given text using TextToSpeech functionality, + * with the same language and country codes that were previously used, or the default language + * and country. + * + * @param text the text to be spoken + */ + void speak(String text); + + /** + * Directs the Driver Station device to speak the given text using TextToSpeech functionality, + * with the given language and country codes. + * + * @param text the text to be spoken + * @param languageCode an ISO 639 alpha-2 or alpha-3 language code, or a language subtag up to + * 8 characters in length + * @param countryCode an ISO 3166 alpha-2 country code, or a UN M.49 numeric-3 area code + */ + void speak(String text, String languageCode, String countryCode); + //---------------------------------------------------------------------------------------------- // Transmission //---------------------------------------------------------------------------------------------- @@ -339,7 +377,7 @@ interface Line /** * Instances of {@link Item} represent an item of data on the drive station telemetry display. * - * @see {@link #addData(String, Object)} + * @see #addData(String, Object) */ interface Item { @@ -404,7 +442,7 @@ interface Item * @see #clear() * @see #isRetained() */ - Item setRetained(Boolean retained); + Item setRetained(@Nullable Boolean retained); /** * Returns whether the item is to be retained in a clear() operation. @@ -498,6 +536,30 @@ interface Item */ void setCaptionValueSeparator(String captionValueSeparator); + enum DisplayFormat + { + CLASSIC, // What you've all come to know and love (or not) since 2015 + MONOSPACE, // Same as classic, except uses a monospaced font so you can column align data + HTML; // Allows use of a subset of HTML tags, enabling "rich text" display (e.g. color & size) + } + + /** + * Sets the telemetry display format on the Driver Station. See the comments on {@link DisplayFormat}. + * + * @param displayFormat the telemetry display format the Driver Station should use + */ + void setDisplayFormat(DisplayFormat displayFormat); + + /** + * Sets the number of decimal places for Double and Float + * + * @param minDecimalPlaces - the minimum number of places to show when Double or Float is passed in without a Format + * @param maxDecimalPlaces - the maximum number of places to show when Double or Float is passed in without a Format + */ + default void setNumDecimalPlaces(int minDecimalPlaces, int maxDecimalPlaces){ + // does nothing just so we don't break existing Telemetry + } + //---------------------------------------------------------------------------------------------- // Properties //---------------------------------------------------------------------------------------------- @@ -516,9 +578,9 @@ interface Log enum DisplayOrder { NEWEST_FIRST, OLDEST_FIRST } /** - * Returns the maximum number of lines which will be retained in a {@link #log()()} and + * Returns the maximum number of lines which will be retained in a {@link #log()} and * shown on the driver station display. - * @return the maximum number of lines which will be retained in a {@link #log()()} + * @return the maximum number of lines which will be retained in a {@link #log()} * @see #setCapacity(int) */ int getCapacity(); @@ -567,4 +629,4 @@ enum DisplayOrder { NEWEST_FIRST, OLDEST_FIRST } * @see Log#addData(String, Object) */ Log log(); -} \ No newline at end of file +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/ColumnMajorMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/ColumnMajorMatrixF.java new file mode 100644 index 00000000..1d6f5f22 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/ColumnMajorMatrixF.java @@ -0,0 +1,56 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link ColumnMajorMatrixF} is a dense matrix whose entries are arranged in + * column-major order. + * @see Row Major Order + */ +public abstract class ColumnMajorMatrixF extends DenseMatrixF +{ + public ColumnMajorMatrixF(int nRows, int nCols) + { + super(nRows, nCols); + } + + @Override protected int indexFromRowCol(int row, int col) + { + return col * numRows + row; + } + + @Override public VectorF toVector() + { + return new VectorF(this.getData()); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/ColumnMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/ColumnMatrixF.java new file mode 100644 index 00000000..79e845f8 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/ColumnMatrixF.java @@ -0,0 +1,62 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link ColumnMatrixF} is a matrix that converts a VectorF into a 1xn matrix + */ +public class ColumnMatrixF extends MatrixF +{ + VectorF vector; + + public ColumnMatrixF(VectorF vector) + { + super(vector.length(), 1); + this.vector = vector; + } + + @Override public float get(int row, int col) + { + return this.vector.get(row); + } + + @Override public void put(int row, int col, float value) + { + this.vector.put(row, value); + } + + @Override public MatrixF emptyMatrix(int numRows, int numCols) + { + return new GeneralMatrixF(numRows, numCols); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/DenseMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/DenseMatrixF.java new file mode 100644 index 00000000..396bd97a --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/DenseMatrixF.java @@ -0,0 +1,75 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link DenseMatrixF} is a matrix of floats whose storage is a contiguous float[] array. It may + * logically be ranged arranged either in row or column major order. + * + * @see MatrixF + * @see RowMajorMatrixF + * @see ColumnMajorMatrixF + * @see SliceMatrixF + */ +public abstract class DenseMatrixF extends MatrixF +{ + protected DenseMatrixF(int nRows, int nCols) + { + super(nRows, nCols); + } + + @Override public float get(int row, int col) + { + return getData()[indexFromRowCol(row, col)]; + } + + @Override public void put(int row, int col, float value) + { + getData()[indexFromRowCol(row, col)] = value; + } + + /** + * Returns the contiguous array of floats which is the storage for this matrix + * @return the contiguous array of floats which is the storage for this matrix + */ + public abstract float[] getData(); + + /** + * Given a row and column index into the matrix, returns the corresponding index + * into the underlying float[] array. + * @param row the row whose index is desired + * @param col the column whose index is desired + * @return the index of (row,col) in the data returned by {@link #getData()} + */ + protected abstract int indexFromRowCol(int row, int col); +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/GeneralMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/GeneralMatrixF.java new file mode 100644 index 00000000..3e86df07 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/GeneralMatrixF.java @@ -0,0 +1,75 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link GeneralMatrixF} is a concrete matrix implementation that is supported by + * a backing store consisting of an array of floats. The matrix is stored in row-major order. + */ +public class GeneralMatrixF extends RowMajorMatrixF +{ + float[] data; + + public GeneralMatrixF(int numRows, int numCols) + { + super(numRows, numCols); + this.data = new float[numRows * numCols]; + } + + private GeneralMatrixF(int numRows, int numCols, int flag) + { + super(numRows, numCols); + } + + public GeneralMatrixF(int numRows, int numCols, float[] data) + { + super(numRows, numCols); + if (data.length != numRows * numCols) throw dimensionsError(numRows, numCols); + this.data = data; + } + + @Override public float[] getData() + { + return this.data; + } + + @Override public MatrixF emptyMatrix(int numRows, int numCols) + { + return new GeneralMatrixF(numRows, numCols); + } + + public GeneralMatrixF transposed() + { + return (GeneralMatrixF)super.transposed(); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/MatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/MatrixF.java new file mode 100644 index 00000000..bfbd11f3 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/MatrixF.java @@ -0,0 +1,804 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +import android.annotation.SuppressLint; + +import org.firstinspires.ftc.robotcore.external.Const; +import org.firstinspires.ftc.robotcore.external.NonConst; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +import java.util.Arrays; + +/** + * {@link MatrixF} represents a matrix of floats of a defined dimensionality but abstracts + * the means by which a particular element of the matrix is retrieved or updated. {@link MatrixF} + * is an abstract class: it is never instantiated; rather, only instances of its subclasses are + * made. + * + * @see Matrix (mathematics) + * @see Matrix multiplication + * @see OpenGLMatrix + * @see GeneralMatrixF + * @see SliceMatrixF + */ +public abstract class MatrixF +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + protected int numRows; + protected int numCols; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + /** + * Creates a matrix containing the indicated number of rows and columns. + */ + public MatrixF(int numRows, int numCols) + { + this.numRows = numRows; + this.numCols = numCols; + if (numRows <= 0 || numCols <= 0) throw dimensionsError(); + } + + /** + * Returns a matrix which a submatrix of the receiver. + * @param row the row in the receiver at which the submatrix is to start + * @param col the column in the receiver at which the submatrix is to start + * @param numRows the number of rows in the submatrix + * @param numCols the number of columns in the submatrix + * @return the newly created submatrix + * @see #slice(int, int) + */ + @Const public SliceMatrixF slice(int row, int col, int numRows, int numCols) + { + return new SliceMatrixF(this, row, col, numRows, numCols); + } + + /** + * Returns a matrix which is a submatrix of the receiver starting at (0,0) + * @param numRows the number of rows in the submatrix + * @param numCols the number of columns in the submatrix + * @return the newly created submatrix + * @see #slice(int, int, int, int) + */ + @Const public SliceMatrixF slice(int numRows, int numCols) + { + return slice(0,0, numRows, numCols); + } + + /** + * Returns an identity matrix of the indicated dimension. An identity matrix is zero + * everywhere except on the diagonal, where it is one. + * @param dim the size of the indentity matrix to return + * @return the new identity matrix + */ + public static MatrixF identityMatrix(int dim) + { + return diagonalMatrix(dim, 1f); + } + + /** + * Returns a new matrix which is zero everywhere except on the diagonal, where it has + * an indicated value. + * @param dim the size of the matrix to return + * @param scale the value to place on its diagonal + * @return the new matrix + */ + public static MatrixF diagonalMatrix(int dim, float scale) + { + GeneralMatrixF result = new GeneralMatrixF(dim, dim); + for (int i = 0; i < dim; i++) + { + result.put(i,i, scale); + } + return result; + } + + /** + * Returns a new matrix which is zero everywhere, except on the diagonal, where its + * values are taken from an indicated vector + * @param vector the values to place on the diagonal + * @return the new matrix + */ + public static MatrixF diagonalMatrix(VectorF vector) + { + int dim = vector.length(); + GeneralMatrixF result = new GeneralMatrixF(dim, dim); + for (int i = 0; i < dim; i++) + { + result.put(i,i, vector.get(i)); + } + return result; + } + + /** + * Returns a new empty matrix of the indicated dimensions. If a specific implementation + * associated with the receiver can be used with these dimensions, then such is used; otherwise + * a general matrix implementation will be used. + * @return a new empty matrix of the indicated dimensions + * @see OpenGLMatrix#emptyMatrix(int, int) + */ + @Const public abstract MatrixF emptyMatrix(int numRows, int numCols); + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + /** + * Returns the number of rows in this matrix + * @return the number of rows in this matrix + */ + @Const public int numRows() { return this.numRows; } + + /** + * Returns the number of columns in this matrix + * @return the number of columns in this matrix + */ + @Const public int numCols() { return this.numCols; } + + /** + * Returns a particular element of this matrix + * @param row the index of the row of the element to return + * @param col the index of the column of the element to return + * @return the element at the indicated row and column + * @see #put(int, int, float) + */ + @Const public abstract float get(int row, int col); + + /** + * Updates a particular element of this matrix + * @param row the index of the row of the element to update + * @param col the index of the column of the element to update + * @param value the new value for the indicated element + */ + @NonConst public abstract void put(int row, int col, float value); + + /** + * Returns a vector containing data of a particular row of the receiver. + * @param row the row to extract + * @return a vector containing the data of the indicated row + */ + @Const public VectorF getRow(int row) + { + VectorF result = VectorF.length(this.numCols); + for (int j = 0; j < numCols; j++) + { + result.put(j, this.get(row, j)); + } + return result; + } + + /** + * Returns a vector containing data of a particular column of the receiver. + * @param col the column to extract + * @return a vector containing data of the indicated column + */ + @Const public VectorF getColumn(int col) + { + VectorF result = VectorF.length(this.numRows); + for (int i = 0; i < numRows; i++) + { + result.put(i, this.get(i, col)); + } + return result; + } + + @Const @Override public String toString() + { + StringBuilder result = new StringBuilder(); + result.append("{"); + for (int i = 0; i < this.numRows; i++) + { + if (i > 0) result.append(","); + result.append("{"); + for (int j = 0; j < this.numCols; j++) + { + if (j > 0) result.append(","); + result.append(String.format("%.3f", this.get(i,j))); + } + result.append("}"); + } + result.append("}"); + return result.toString(); + } + + //---------------------------------------------------------------------------------------------- + // Transformation matrix operations + //---------------------------------------------------------------------------------------------- + + /** + * Transforms the vector according to this matrix interpreted as a transformation matrix. + * Conversion to homogeneous + * coordinates is automatically provided. + * @param him the 3D coordinate or 3D homogeneous coordinate that is to be transformed + * @return the normalized homogeneous coordinate resulting from the transformation. + * + * @see Homogeneous coordinates + * @see Transformation Matrix + * @see VectorF#normalized3D() + */ + @Const public VectorF transform(VectorF him) + { + him = adaptHomogeneous(him); + return this.multiplied(him).normalized3D(); + } + + /** + * Automatically adapts vectors to and from homogeneous coordinates according to the + * size of the receiver matrix. + * @see #transform(VectorF) + * @see Homogeneous coordinates + */ + @Const protected VectorF adaptHomogeneous(VectorF him) + { + if (this.numCols == 4) + { + if (him.length() == 3) + { + float[] newData = Arrays.copyOf(him.getData(), 4); + newData[3] = 1f; + return new VectorF(newData); + } + } + else if (this.numCols == 3) + { + if (him.length() == 4) + { + return new VectorF(Arrays.copyOf(him.normalized3D().getData(),3)); + } + } + return him; + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. This should only be invoked on + * a matrix which is a transformation matrix. + * + * We report here using an extrinsic angle reference, meaning that all three angles are + * rotations in the (fixed) field coordinate system, as this is perhaps easiest to + * conceptually understand. And we use an angle order of XYZ, which results in the Z + * angle, being applied last (after X and Y rotations) and so representing the robot's + * heading on the field, which is often what is of most interest in robot navigation. + * + * @return a description of the angles represented by this transformation matrix. + * @see #formatAsTransform(AxesReference, AxesOrder, AngleUnit) + * @see Transformation Matrix + */ + public String formatAsTransform() + { + return formatAsTransform(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES); + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. This should only be invoked on + * a matrix which is a transformation matrix. + * + * @param axesReference the reference frame of the angles to use in reporting the transformation + * @param axesOrder the order of the angles to use in reporting the transformation + * @param unit the angular unit to use in reporting the transformation + * @return a description of the angles represented by this transformation matrix. + * @see #formatAsTransform() + * @see Transformation Matrix + */ + public String formatAsTransform(AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit) + { + /** + * An easy way to understand what a transform does is to look at the location + * to which it transforms the origin of the coordinate system. Calling getTranslation() + * carries out an equivalent computation as it extracts the translational aspect. + */ + VectorF translation = this.getTranslation(); + + /** + * Figure out in which direction we'd be looking after the transformation. Note that + * the decomposition of a transformation into orientation angles can be subtle. See + * {@link Orientation} for a full discussion. + */ + Orientation orientation = Orientation.getOrientation(this, axesReference, axesOrder, unit); + + return String.format("%s %s", orientation.toString(), translation.toString()); + } + + //---------------------------------------------------------------------------------------------- + // Matrix operations + //---------------------------------------------------------------------------------------------- + + /** + * Returns a matrix which is the transposition of the receiver matrix. + * @return a matrix which is the transposition of the receiver matrix. + */ + @Const public MatrixF transposed() + { + MatrixF result = this.emptyMatrix(this.numCols, this.numRows); + for (int i = 0; i < result.numRows; i++) + { + for (int j = 0; j < result.numCols; j++) + { + result.put(i,j, this.get(j,i)); + } + } + return result; + } + + /** + * Updates the receiver to be the product of itself and another matrix. + * @param him the matrix with which the receiver is to be multiplied. + */ + @NonConst public void multiply(MatrixF him) + { + /** + * If we multiply C = A x B, the dimensions work out as C(i x k) = A(i x j) B(j x k). + * If A and C are the same matrix, we have j==k; that is, B must be square. + */ + if (this.numCols == him.numRows) + { + if (him.numRows == him.numCols) + { + MatrixF temp = this.multiplied(him); + + // Copy the matrix back + for (int i = 0; i < this.numRows; i++) + { + for (int j = 0; j < this.numCols; j++) + { + this.put(i,j, temp.get(i,j)); + } + } + } + else + throw dimensionsError(); + } + else + throw dimensionsError(); + } + + /** + * Returns a matrix which is the multiplication of the recevier with another matrix. + * @param him the matrix with which the receiver is to be multiplied. + * @return a matrix which is the product of the two matrices + */ + @Const public MatrixF multiplied(MatrixF him) + { + if (this.numCols == him.numRows) + { + MatrixF result = this.emptyMatrix(this.numRows, him.numCols); + for (int i = 0; i < result.numRows; i++) + { + for (int j = 0; j < result.numCols; j++) + { + float sum = 0f; + for (int k = 0; k < this.numCols; k++) + { + sum += this.get(i, k) * him.get(k, j); + } + result.put(i,j,sum); + } + } + return result; + } + else + throw dimensionsError(); + } + + /** + * Returns a new matrix in which all the entries of the receiver have been scaled + * by an indicated value. + * @param scale the factor with which to scale each entry of the receiver + * @return the new, scaled matrix + */ + @Const public MatrixF multiplied(float scale) + { + MatrixF result = this.emptyMatrix(this.numCols, this.numRows); + for (int i = 0; i < result.numRows; i++) + { + for (int j = 0; j < result.numCols; j++) + { + result.put(i,j, this.get(i,j) * scale); + } + } + return result; + } + + @NonConst public void multiply(float scale) + { + for (int i = 0; i < this.numRows; i++) + { + for (int j = 0; j < this.numCols; j++) + { + this.put(i,j, this.get(i,j) * scale); + } + } + } + + /** + * Multiplies the receiver by the indicated vector, considered as a column matrix. + * @param him the vector with which the receiver is to be multiplied + * @return a matrix which is the product of the receiver and the vector + */ + @Const public VectorF multiplied(VectorF him) + { + return this.multiplied(new ColumnMatrixF(him)).toVector(); + } + + @NonConst public void multiply(VectorF him) + { + VectorF result = this.multiplied(new ColumnMatrixF(him)).toVector(); + for (int i = 0; i < result.length(); i++) + { + this.put(i,0, result.get(i)); + } + } + + /** + * Multiplies the receiver by the indicated vector, considered as a column matrix. + * @param him the vector with which the receiver is to be multiplied + * @return a matrix which is the product of the receiver and the vector + */ + @Const public VectorF multiplied(float[] him) + { + return this.multiplied(new VectorF(him)); + } + + @NonConst public void multiply(float[] him) + { + VectorF result = this.multiplied(new VectorF(him)); + for (int i = 0; i < result.length(); i++) + { + this.put(i,0, result.get(i)); + } + } + + /** + * If the receiver is one-dimensional in one of its dimensions, returns a vector + * containing the data of the receiver; otherwise, an exception is thrown. + * @return a vector containing the data of the receiver + */ + @Const public VectorF toVector() + { + if (this.numCols == 1) + { + VectorF result = VectorF.length(this.numRows); + for (int i = 0; i < this.numRows; i++) + { + result.put(i, this.get(i,0)); + } + return result; + } + else if (this.numRows == 1) + { + VectorF result = VectorF.length(this.numCols); + for (int j = 0; j < this.numCols; j++) + { + result.put(j, this.get(0,j)); + } + return result; + } + else + throw dimensionsError(); + } + + /** + * Returns a new matrix whose elements are the sum of the corresponding elements of + * the receiver and the addend + * @param addend the matrix which is to be added to the receiver + * @return the new matrix + */ + @Const public MatrixF added(MatrixF addend) + { + if (this.numRows==addend.numRows && this.numCols==addend.numCols) + { + MatrixF result = this.emptyMatrix(this.numRows, this.numCols); + for (int i = 0; i < result.numRows; i++) + { + for (int j = 0; j < result.numCols; j++) + { + result.put(i,j, this.get(i,j) + addend.get(i,j)); + } + } + return result; + } + else + throw dimensionsError(); + } + + /** + * Adds a matrix, in place, to the receiver + * @param addend the matrix which is to be added to the receiver + */ + @NonConst public void add(MatrixF addend) + { + if (this.numRows==addend.numRows && this.numCols==addend.numCols) + { + for (int i = 0; i < this.numRows; i++) + { + for (int j = 0; j < this.numCols; j++) + { + this.put(i,j, this.get(i,j) + addend.get(i,j)); + } + } + } + else + throw dimensionsError(); + } + + /** + * Returns a new matrix whose elements are the difference of the corresponding elements of + * the receiver and the subtrahend + * @param subtrahend the matrix which is to be subtracted from the receiver + * @return the new matrix + */ + @Const public MatrixF subtracted(MatrixF subtrahend) + { + if (this.numRows==subtrahend.numRows && this.numCols==subtrahend.numCols) + { + MatrixF result = this.emptyMatrix(this.numRows, this.numCols); + for (int i = 0; i < result.numRows; i++) + { + for (int j = 0; j < result.numCols; j++) + { + result.put(i,j, this.get(i,j) - subtrahend.get(i,j)); + } + } + return result; + } + else + throw dimensionsError(); + } + + /** + * Subtracts a matrix, in place, from the receiver. + * @param subtrahend the matrix which is to be subtracted from the receiver + */ + @NonConst public void subtract(MatrixF subtrahend) + { + if (this.numRows==subtrahend.numRows && this.numCols==subtrahend.numCols) + { + for (int i = 0; i < this.numRows; i++) + { + for (int j = 0; j < this.numCols; j++) + { + this.put(i,j, this.get(i,j) - subtrahend.get(i,j)); + } + } + } + else + throw dimensionsError(); + } + + /** @see #added(MatrixF) */ + @Const public MatrixF added(VectorF him) + { + return this.added(new ColumnMatrixF(him)); + } + /** @see #added(VectorF) */ + @Const public MatrixF added(float[] him) + { + return this.added(new VectorF(him)); + } + /** @see #subtracted(MatrixF) */ + @Const public MatrixF subtracted(VectorF him) + { + return this.subtracted(new ColumnMatrixF(him)); + } + /** @see #subtracted(VectorF) */ + @Const public MatrixF subtracted(float[] him) + { + return this.subtracted(new VectorF(him)); + } + + /** @see #add(MatrixF) */ + @NonConst public void add(VectorF him) + { + this.add(new ColumnMatrixF(him)); + } + /** @see #add(VectorF) */ + @NonConst public void add(float[] him) + { + this.add(new VectorF(him)); + } + /** @see #subtract(MatrixF) */ + @NonConst public void subtract(VectorF him) + { + this.subtract(new ColumnMatrixF(him)); + } + /** @see #subtract(VectorF) */ + @NonConst public void subtract(float[] him) + { + this.subtract(new VectorF(him)); + } + + //---------------------------------------------------------------------------------------------- + // Transformations + //---------------------------------------------------------------------------------------------- + + /** + * Assumes that the receiver is non-perspective transformation matrix. Returns the translation + * component of the transformation. + * @return the translation component of the transformation + */ + @Const public VectorF getTranslation() + { + return this.getColumn(3).normalized3D(); + } + + //---------------------------------------------------------------------------------------------- + // Utility + //---------------------------------------------------------------------------------------------- + + protected RuntimeException dimensionsError() + { + return dimensionsError(this.numRows, this.numCols); + } + + @SuppressLint("DefaultLocale") + protected static RuntimeException dimensionsError(int numRows, int numCols) + { + return new IllegalArgumentException(String.format("matrix dimensions are incorrect: rows=%d cols=%d", numRows, numCols)); + } + + //---------------------------------------------------------------------------------------------- + // Inverses (at end because of verbosity) + //---------------------------------------------------------------------------------------------- + + /** + * Returns a matrix which is the matrix-multiplication inverse of the receiver. + * @return a matrix which is the matrix-multiplication inverse of the receiver + */ + @Const public MatrixF inverted() + { + // Algorithms were generated with the help of Mathematica: general nxn matrices with symbolic + // (instead of numeric) entries were defined, their inverse symbolically computed, then + // automatically transcribed to Java. + + if (this.numRows != this.numCols) throw dimensionsError(); + + if (this.numRows == 4) + { + MatrixF result = this.emptyMatrix(4,4); + + final float m00=get(0,0), m01=get(0,1), m02=get(0,2), m03=get(0,3); + final float m10=get(1,0), m11=get(1,1), m12=get(1,2), m13=get(1,3); + final float m20=get(2,0), m21=get(2,1), m22=get(2,2), m23=get(2,3); + final float m30=get(3,0), m31=get(3,1), m32=get(3,2), m33=get(3,3); + + final float denom = m00 * m11 * m22 * m33 + + m00 * m12 * m23 * m31 + + m00 * m13 * m21 * m32 + + m01 * m10 * m23 * m32 + + m01 * m12 * m20 * m33 + + m01 * m13 * m22 * m30 + + m02 * m10 * m21 * m33 + + m02 * m11 * m23 * m30 + + m02 * m13 * m20 * m31 + + m03 * m10 * m22 * m31 + + m03 * m11 * m20 * m32 + + m03 * m12 * m21 * m30 + - m01 * m10 * m22 * m33 + - m00 * m12 * m21 * m33 + - m02 * m11 * m20 * m33 + - m00 * m11 * m23 * m32 + - m03 * m10 * m21 * m32 + - m01 * m13 * m20 * m32 + - m02 * m10 * m23 * m31 + - m00 * m13 * m22 * m31 + - m03 * m12 * m20 * m31 + - m01 * m12 * m23 * m30 + - m03 * m11 * m22 * m30 + - m02 * m13 * m21 * m30; + + result.put(0, 0, (m11 * m22 * m33 + m12 * m23 * m31 + m13 * m21 * m32 - m12 * m21 * m33 - m11 * m23 * m32 - m13 * m22 * m31) / denom); + result.put(0, 1, (m01 * m23 * m32 + m02 * m21 * m33 + m03 * m22 * m31 - m01 * m22 * m33 - m03 * m21 * m32 - m02 * m23 * m31) / denom); + result.put(0, 2, (m01 * m12 * m33 + m02 * m13 * m31 + m03 * m11 * m32 - m02 * m11 * m33 - m01 * m13 * m32 - m03 * m12 * m31) / denom); + result.put(0, 3, (m01 * m13 * m22 + m02 * m11 * m23 + m03 * m12 * m21 - m01 * m12 * m23 - m03 * m11 * m22 - m02 * m13 * m21) / denom); + result.put(1, 0, (m10 * m23 * m32 + m12 * m20 * m33 + m13 * m22 * m30 - m10 * m22 * m33 - m13 * m20 * m32 - m12 * m23 * m30) / denom); + result.put(1, 1, (m00 * m22 * m33 + m02 * m23 * m30 + m03 * m20 * m32 - m02 * m20 * m33 - m00 * m23 * m32 - m03 * m22 * m30) / denom); + result.put(1, 2, (m00 * m13 * m32 + m02 * m10 * m33 + m03 * m12 * m30 - m00 * m12 * m33 - m03 * m10 * m32 - m02 * m13 * m30) / denom); + result.put(1, 3, (m00 * m12 * m23 + m02 * m13 * m20 + m03 * m10 * m22 - m02 * m10 * m23 - m00 * m13 * m22 - m03 * m12 * m20) / denom); + result.put(2, 0, (m10 * m21 * m33 + m11 * m23 * m30 + m13 * m20 * m31 - m11 * m20 * m33 - m10 * m23 * m31 - m13 * m21 * m30) / denom); + result.put(2, 1, (m00 * m23 * m31 + m01 * m20 * m33 + m03 * m21 * m30 - m00 * m21 * m33 - m03 * m20 * m31 - m01 * m23 * m30) / denom); + result.put(2, 2, (m00 * m11 * m33 + m01 * m13 * m30 + m03 * m10 * m31 - m01 * m10 * m33 - m00 * m13 * m31 - m03 * m11 * m30) / denom); + result.put(2, 3, (m00 * m13 * m21 + m01 * m10 * m23 + m03 * m11 * m20 - m00 * m11 * m23 - m03 * m10 * m21 - m01 * m13 * m20) / denom); + result.put(3, 0, (m10 * m22 * m31 + m11 * m20 * m32 + m12 * m21 * m30 - m10 * m21 * m32 - m12 * m20 * m31 - m11 * m22 * m30) / denom); + result.put(3, 1, (m00 * m21 * m32 + m01 * m22 * m30 + m02 * m20 * m31 - m01 * m20 * m32 - m00 * m22 * m31 - m02 * m21 * m30) / denom); + result.put(3, 2, (m00 * m12 * m31 + m01 * m10 * m32 + m02 * m11 * m30 - m00 * m11 * m32 - m02 * m10 * m31 - m01 * m12 * m30) / denom); + result.put(3, 3, (m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m01 * m10 * m22 - m00 * m12 * m21 - m02 * m11 * m20) / denom); + + return result; + } + + if (this.numRows == 3) + { + MatrixF result = this.emptyMatrix(3,3); + + final float m00=get(0,0), m01=get(0,1), m02=get(0,2); + final float m10=get(1,0), m11=get(1,1), m12=get(1,2); + final float m20=get(2,0), m21=get(2,1), m22=get(2,2); + + final float denom = m00 * m11 * m22 + + m01 * m12 * m20 + + m02 * m10 * m21 + - m01 * m10 * m22 + - m00 * m12 * m21 + - m02 * m11 * m20; + + result.put(0, 0, (m11 * m22 - m12 * m21) / denom); + result.put(0, 1, (m02 * m21 - m01 * m22) / denom); + result.put(0, 2, (m01 * m12 - m02 * m11) / denom); + result.put(1, 0, (m12 * m20 - m10 * m22) / denom); + result.put(1, 1, (m00 * m22 - m02 * m20) / denom); + result.put(1, 2, (m02 * m10 - m00 * m12) / denom); + result.put(2, 0, (m10 * m21 - m11 * m20) / denom); + result.put(2, 1, (m01 * m20 - m00 * m21) / denom); + result.put(2, 2, (m00 * m11 - m01 * m10) / denom); + + return result; + } + + if (this.numRows == 2) + { + MatrixF result = this.emptyMatrix(2,2); + + final float m00=get(0,0), m01=get(0,1); + final float m10=get(1,0), m11=get(1,1); + + final float denom = m00 * m11 - m01 * m10; + + result.put(0, 0, (m11) / denom); + result.put(0, 1, (-m01) / denom); + result.put(1, 0, (-m10) / denom); + result.put(1, 1, (m00) / denom); + + return result; + } + + if (this.numRows == 1) + { + MatrixF result = this.emptyMatrix(1,1); + result.put(0,0, 1 / get(0,0)); + return result; + } + + throw dimensionsError(); // really NYI: we haven't bothered to code other cases + } + +} \ No newline at end of file diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/OpenGLMatrix.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/OpenGLMatrix.java new file mode 100644 index 00000000..277867f9 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/OpenGLMatrix.java @@ -0,0 +1,264 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +import android.opengl.Matrix; + +import org.firstinspires.ftc.robotcore.external.Const; +import org.firstinspires.ftc.robotcore.external.NonConst; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +/** + * An {@link OpenGLMatrix} is a 4x4 matrix commonly used as a transformation matrix for 3D + * homogeneous coordinates. The data layout of an {@link OpenGLMatrix} is used heavily in the + * OpenGL high performance graphics standard. + * + * @see Homogenous coordinates + * @see Transformation Matrix + * @see android.opengl.Matrix + * @see Matrix + */ +public class OpenGLMatrix extends ColumnMajorMatrixF +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + float[] data; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public OpenGLMatrix() + { + super(4,4); + this.data = new float[4*4]; + Matrix.setIdentityM(this.data, 0); + } + + public OpenGLMatrix(float[] data) + { + super(4,4); + this.data = data; + if (this.data.length != 4*4) throw dimensionsError(); + } + + /** + * Constructs an OpenGL matrix whose values are initialized from the other matrix. + * The other matrix must have dimensions at most 4x4. + * @param him the matrix from which to initialize our own data + */ + public OpenGLMatrix(MatrixF him) + { + this(); + if (him.numRows > 4 || him.numCols > 4) throw him.dimensionsError(); + for (int i = 0; i < Math.min(4,him.numRows); i++) + { + for (int j = 0; j < Math.min(4,him.numCols); j++) + { + this.put(i,j, him.get(i,j)); + } + } + } + + @Override public MatrixF emptyMatrix(int numRows, int numCols) + { + if (numRows==4 && numCols==4) + return new OpenGLMatrix(); + else + return new GeneralMatrixF(numRows, numCols); + } + + /** + * Creates a matrix for rotation by the indicated angle around the indicated vector. + */ + public static OpenGLMatrix rotation(AngleUnit angleUnit, float angle, float dx, float dy, float dz) + { + float[] data = new float[16]; + Matrix.setRotateM(data, 0, angleUnit.toDegrees(angle), dx, dy, dz); + return new OpenGLMatrix(data); + } + + /** + * Creates a matrix for a rotation specified by three successive rotation angles. + * @see Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float) + */ + public static OpenGLMatrix rotation(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float first, float second, float third) + { + OpenGLMatrix rotation = Orientation.getRotationMatrix(axesReference, axesOrder, angleUnit, first, second, third); + return identityMatrix().multiplied(rotation); + } + public static OpenGLMatrix translation(float dx, float dy, float dz) + { + OpenGLMatrix result = new OpenGLMatrix(); + result.translate(dx, dy, dz); + return result; + } + public static OpenGLMatrix identityMatrix() + { + return new OpenGLMatrix(); + } + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + @Override public float[] getData() + { + return this.data; + } + + //---------------------------------------------------------------------------------------------- + // Transformation matrix operations (in-place). These methods all return the receiver + // in order to facilitate chaining. + // + // Note that these are some of the very view matrix operations that update-in-place rather than + // returning a new matrix and leaving the receiver unmodified. Care must thus be taken to avoid + // sharing the data of this matrix (using getData()) with other matrix-related objects and then + // subsequently modifying this matrix. + //---------------------------------------------------------------------------------------------- + + @NonConst public void scale(float scaleX, float scaleY, float scaleZ) + { + Matrix.scaleM(this.data, 0, scaleX, scaleY, scaleZ); + } + @NonConst public void scale(float scale) + { + this.scale(scale, scale, scale); + } + @NonConst public void translate(float dx, float dy, float dz) + { + Matrix.translateM(this.data, 0, dx, dy, dz); + } + @NonConst public void rotate(AngleUnit angleUnit, float angle, float dx, float dy, float dz) + { + Matrix.rotateM(this.data, 0, angleUnit.toDegrees(angle), dx, dy, dz); + } + @NonConst public void rotate(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float first, float second, float third) + { + OpenGLMatrix rotation = Orientation.getRotationMatrix(axesReference, axesOrder, angleUnit, first, second, third); + this.data = this.multiplied(rotation).getData(); + } + + //---------------------------------------------------------------------------------------------- + // Transformation matrix operations + //---------------------------------------------------------------------------------------------- + + @Const public OpenGLMatrix scaled(float scaleX, float scaleY, float scaleZ) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.scaleM(result.data, 0, this.data, 0, scaleX, scaleY, scaleZ); + return result; + } + @Const public OpenGLMatrix scaled(float scale) + { + return scaled(scale, scale, scale); + } + @Const public OpenGLMatrix translated(float dx, float dy, float dz) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.translateM(result.data, 0, this.data, 0, dx, dy, dz); + return result; + } + @Const public OpenGLMatrix rotated(AngleUnit angleUnit, float angle, float dx, float dy, float dz) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.rotateM(result.data, 0, this.data, 0, angleUnit.toDegrees(angle), dx, dy, dz); + return result; + } + @Const public OpenGLMatrix rotated(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float first, float second, float third) + { + OpenGLMatrix rotation = Orientation.getRotationMatrix(axesReference, axesOrder, angleUnit, first, second, third); + return this.multiplied(rotation); + } + + //---------------------------------------------------------------------------------------------- + // Matrix operations + //---------------------------------------------------------------------------------------------- + + @Override @Const public OpenGLMatrix inverted() + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.invertM(result.data, 0, this.data, 0); + return result; + } + + @Override @Const public OpenGLMatrix transposed() + { + return (OpenGLMatrix)super.transposed(); + } + + @Const public OpenGLMatrix multiplied(OpenGLMatrix him) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.multiplyMM(result.data, 0, this.data, 0, him.getData(), 0); + return result; + } + + @Override @Const public MatrixF multiplied(MatrixF him) + { + if (him instanceof OpenGLMatrix) + { + return this.multiplied((OpenGLMatrix)him); + } + else + return super.multiplied(him); + } + + /** + * Updates the receiver to be the product of itself and another matrix. + * @param him the matrix with which the receiver is to be multiplied. + */ + @NonConst public void multiply(OpenGLMatrix him) + { + this.data = this.multiplied(him).getData(); + } + + /** + * Updates the receiver to be the product of itself and another matrix. + * @param him the matrix with which the receiver is to be multiplied. + */ + @Override @NonConst public void multiply(MatrixF him) + { + if (him instanceof OpenGLMatrix) + { + this.multiply((OpenGLMatrix)him); + } + else + super.multiply(him); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/RowMajorMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/RowMajorMatrixF.java new file mode 100644 index 00000000..9a36052a --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/RowMajorMatrixF.java @@ -0,0 +1,57 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link RowMajorMatrixF} is a dense matrix whose entries are arranged in + * row-major order. + * @see Row Major Order + */ +public abstract class RowMajorMatrixF extends DenseMatrixF +{ + public RowMajorMatrixF(int nRows, int nCols) + { + super(nRows, nCols); + } + + @Override + protected int indexFromRowCol(int row, int col) + { + return row * numCols + col; + } + + @Override public VectorF toVector() + { + return new VectorF(this.getData()); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/RowMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/RowMatrixF.java new file mode 100644 index 00000000..86b5cee8 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/RowMatrixF.java @@ -0,0 +1,62 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link ColumnMatrixF} is a matrix that converts a VectorF into a 1xn matrix + */ +public class RowMatrixF extends MatrixF +{ + VectorF vector; + + public RowMatrixF(VectorF vector) + { + super(1, vector.length()); + this.vector = vector; + } + + @Override public float get(int row, int col) + { + return this.vector.get(col); + } + + @Override public void put(int row, int col, float value) + { + this.vector.put(col, value); + } + + @Override public MatrixF emptyMatrix(int numRows, int numCols) + { + return new GeneralMatrixF(numRows, numCols); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/SliceMatrixF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/SliceMatrixF.java new file mode 100644 index 00000000..d85b9d9e --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/SliceMatrixF.java @@ -0,0 +1,90 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +/** + * A {@link SliceMatrixF} is a matrix whose implementation is a submatrix of some other matrix. + */ +public class SliceMatrixF extends MatrixF +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + protected MatrixF matrix; + protected int row; + protected int col; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + /** + * Creates a {@link SliceMatrixF} based on the indicated matrix whose upper left corner is at + * (row, col) of that matrix and whose size is numRows x numCols. + * @param matrix the matrix we are to take a slice of + * @param row the row in matrix in which the slice is to begin + * @param col the column in matrix in which the slice is to begin + * @param numRows the number of rows that the slice should be + * @param numCols the number of columns that the slice should be + */ + public SliceMatrixF(MatrixF matrix, int row, int col, int numRows, int numCols) + { + super(numRows, numCols); + this.matrix = matrix; + this.row = row; + this.col = col; + + if (row + numRows >= matrix.numRows) throw dimensionsError(); + if (col + numCols >= matrix.numCols) throw dimensionsError(); + } + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + @Override public float get(int row, int col) + { + return this.matrix.get(this.row + row, this.col + col); + } + + @Override public void put(int row, int col, float value) + { + this.matrix.put(this.row + row, this.col + col, value); + } + + @Override public MatrixF emptyMatrix(int numRows, int numCols) + { + return this.matrix.emptyMatrix(numRows, numCols); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/VectorF.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/VectorF.java new file mode 100644 index 00000000..67ae41ba --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/matrices/VectorF.java @@ -0,0 +1,318 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +import android.annotation.SuppressLint; + +import org.firstinspires.ftc.robotcore.external.Const; +import org.firstinspires.ftc.robotcore.external.NonConst; + +/** + * A {@link VectorF} represents a single-dimensional vector of floats. It is not a matrix, + * but can easily be converted into either a {@link RowMatrixF} or a {@link ColumnMatrixF} should + * that be desired. That said, vectors can be multiplied by matrices to their left (or right); this + * is commonly used to transform a set of coordinates (in the vector) by a transformation matrix. + * + * @see MatrixF + * @see RowMatrixF + * @see ColumnMatrixF + */ +public class VectorF +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + protected float[] data; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + /** + * Creates a new vector of the indicated length. The vector will contain zeros. + * @param length the length of the new vector to return + * @return the newly created vector + */ + public static VectorF length(int length) + { + return new VectorF(new float[length]); + } + + public VectorF(float[] data) + { + this.data = data; + } + + public VectorF(float x) + { + this.data = new float[1]; + this.data[0] = x; + } + + public VectorF(float x, float y) + { + this.data = new float[2]; + this.data[0] = x; + this.data[1] = y; + } + + public VectorF(float x, float y, float z) + { + this.data = new float[3]; + this.data[0] = x; + this.data[1] = y; + this.data[2] = z; + } + + public VectorF(float x, float y, float z, float w) + { + this.data = new float[4]; + this.data[0] = x; + this.data[1] = y; + this.data[2] = z; + this.data[3] = w; + } + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + @Const public float[] getData() + { + return this.data; + } + + @Const public int length() + { + return this.data.length; + } + + @Const public float get(int index) + { + return this.data[index]; + } + + @NonConst public void put(int index, float value) + { + this.data[index] = value; + } + + @Override public String toString() + { + StringBuilder result = new StringBuilder(); + result.append("{"); + for (int i = 0; i < this.length(); i++) + { + if (i > 0) result.append(" "); + result.append(String.format("%.2f", this.data[i])); + } + result.append("}"); + return result.toString(); + } + + //---------------------------------------------------------------------------------------------- + // Transformation matrix operations + //---------------------------------------------------------------------------------------------- + + /** + * Consider this vector as a 3D coordinate or 3D homogeneous coordinate, and, if the + * latter, return its normalized form. In either case, the result is of length three, and + * contains coordinate values for x, y, and z at indices 0, 1, and 2 respectively. + * @return the normalized form of this coordinate vector + * + * @see Homogeneous coordinates + */ + @Const public VectorF normalized3D() + { + if (this.length()==3) + { + return this; + } + else if (this.length()==4) + { + return new VectorF( + this.data[0]/this.data[3], + this.data[1]/this.data[3], + this.data[2]/this.data[3]); + } + else + throw dimensionsError(); + } + + //---------------------------------------------------------------------------------------------- + // Matrix Operations + //---------------------------------------------------------------------------------------------- + + @Const public float magnitude() + { + return (float)Math.sqrt(this.dotProduct(this)); + } + + /** + * Returns the dot product of this vector and another. + * @param him the other vector with whom the dot product is to be formed + * @return the dot product of this vector and another. + * + * @see Dot product + */ + @Const public float dotProduct(VectorF him) + { + if (this.length() == him.length()) + { + float sum = 0; + for (int i = 0; i < this.length(); i++) + { + sum += this.get(i) * him.get(i); + } + return sum; + } + else + throw dimensionsError(); + } + + /** + * Multiplies this vector, taken as a row vector, against the indicated matrix. + */ + @Const public MatrixF multiplied(MatrixF him) + { + return new RowMatrixF(this).multiplied(him); + } + + /** + * Adds this vector, taken as a row vector against, to the indicated matrix. + */ + @Const public MatrixF added(MatrixF addend) + { + return new RowMatrixF(this).added(addend); + } + + @Const public VectorF added(VectorF addend) + { + if (this.length() == addend.length()) + { + VectorF result = VectorF.length(this.length()); + for (int i = 0; i < this.length(); i++) + { + result.put(i, this.get(i) + addend.get(i)); + } + return result; + } + else + throw dimensionsError(); + } + + @NonConst public void add(VectorF addend) + { + if (this.length() == addend.length()) + { + for (int i = 0; i < this.length(); i++) + { + this.put(i, this.get(i) + addend.get(i)); + } + } + else + throw dimensionsError(); + } + + /** + * Subtracts the indicated matrix from this vector, taken as a row vector. + */ + @Const public MatrixF subtracted(MatrixF subtrahend) + { + return new RowMatrixF(this).subtracted(subtrahend); + } + + @Const public VectorF subtracted(VectorF subtrahend) + { + if (this.length() == subtrahend.length()) + { + VectorF result = VectorF.length(this.length()); + for (int i = 0; i < this.length(); i++) + { + result.put(i, this.get(i) - subtrahend.get(i)); + } + return result; + } + else + throw dimensionsError(); + } + + @NonConst public void subtract(VectorF subtrahend) + { + if (this.length() == subtrahend.length()) + { + for (int i = 0; i < this.length(); i++) + { + this.put(i, this.get(i) - subtrahend.get(i)); + } + } + else + throw dimensionsError(); + } + + /** + * Returns a new vector containing the elements of this vector scaled by the indicated factor. + */ + @Const public VectorF multiplied(float scale) + { + VectorF result = VectorF.length(this.length()); + for (int i = 0; i < this.length(); i++) + { + result.put(i, this.get(i) * scale); + } + return result; + } + + @NonConst public void multiply(float scale) + { + for (int i = 0; i < this.length(); i++) + { + this.put(i, this.get(i) * scale); + } + } + + //---------------------------------------------------------------------------------------------- + // Utility + //---------------------------------------------------------------------------------------------- + + protected RuntimeException dimensionsError() + { + return dimensionsError(this.length()); + } + + @SuppressLint("DefaultLocale") protected static RuntimeException dimensionsError(int length) + { + return new IllegalArgumentException(String.format("vector dimensions are incorrect: length=%d", length)); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Acceleration.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Acceleration.java new file mode 100644 index 00000000..da450693 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Acceleration.java @@ -0,0 +1,120 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +import java.util.Locale; + +/** + * Instances of {@link Acceleration} represent the second derivative of {@link Position} over time. This + * is also to say that {@code Position} is a double integration of {@code Acceleration} with respect + * to time. + * + * @see Velocity + * @see Position + */ +public class Acceleration +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + /** The (nominal) acceleration due to Earth's gravity + * The units are in m/s^2 + */ + public static final double earthGravity = 9.80665; + + /** + * The distance units in which this acceleration is expressed. The time unit is always "per second per second". + */ + public DistanceUnit unit; + + public double xAccel; + public double yAccel; + public double zAccel; + + /** + * the time on the System.nanoTime() clock at which the data was acquired. If no + * timestamp is associated with this particular set of data, this value is zero. + */ + public long acquisitionTime; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public Acceleration() + { + this(DistanceUnit.MM, 0, 0, 0, 0); + } + + public Acceleration(DistanceUnit unit, double xAccel, double yAccel, double zAccel, long acquisitionTime) + { + this.unit = unit; + this.xAccel = xAccel; + this.yAccel = yAccel; + this.zAccel = zAccel; + this.acquisitionTime = acquisitionTime; + } + + /** + * Returns an acceleration constructed from measures in units of earth's gravity + * rather than explicit distance units. + */ + public static Acceleration fromGravity(double gx, double gy, double gz, long acquisitionTime) + { + return new Acceleration(DistanceUnit.METER, gx * earthGravity, gy * earthGravity, gz * earthGravity, acquisitionTime); + } + + public Acceleration toUnit(DistanceUnit distanceUnit) + { + if (distanceUnit != this.unit) + { + return new Acceleration(distanceUnit, + distanceUnit.fromUnit(this.unit, xAccel), + distanceUnit.fromUnit(this.unit, yAccel), + distanceUnit.fromUnit(this.unit, zAccel), + this.acquisitionTime); + } + else + return this; + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + @Override public String toString() + { + return String.format(Locale.getDefault(), "(%.3f %.3f %.3f)%s/s^2", xAccel, yAccel, zAccel, unit.toString()); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java new file mode 100644 index 00000000..39150c91 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java @@ -0,0 +1,239 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +/** + * An {@link AngleUnit} represents angles in different units of measure and + * provides utility methods to convert across units. {@link AngleUnit} does not + * maintain angle information information internally, but only helps organize + * and use angle measures that may be maintained separately across various contexts. + *

+ * Angles can be distinguished along (at least) two axes: + *

    + *
  1. the fundamental unit (radians vs degrees)
    2. + *
  2. whether the angular quantity is normalized or not to the range of [-180,+180) degrees
    3. + *
+ * Normalized angles are of most utility when dealing with physical angles, as normalization + * removes ambiguity of representation. In particular, two angles can be compared for equality + * by subtracting them, normalizing, and testing whether the absolute value of the result is + * smaller than some tolerance threshold. This approach neatly handles all cases of cyclical + * wrapping without unexpected discontinuities. + *

+ * Unnormalized angles can be handy when the angular quantity is not a physical angle but some + * related quantity such as an angular velocity or acceleration, where the + * quantity in question lacks the 360-degree cyclical equivalence of a physical angle. + *

+ * {@link AngleUnit} expresses normalized angles, while {@link UnnormalizedAngleUnit} expresses unnormalized ones + *

+ */ +@SuppressWarnings("WeakerAccess") +public enum AngleUnit +{ + DEGREES(0), RADIANS(1); + public final byte bVal; + + protected static final double TwoPi = 2 * Math.PI; + public static final float Pif = (float) Math.PI; + + AngleUnit(int i) + { + bVal = (byte) i; + } + + //---------------------------------------------------------------------------------------------- + // Primitive operations + //---------------------------------------------------------------------------------------------- + + public double fromDegrees(double degrees) + { + switch (this) + { + default: + case RADIANS: return this.normalize(degrees / 180.0 * Math.PI); + case DEGREES: return this.normalize(degrees); + } + } + + public float fromDegrees(float degrees) + { + switch (this) + { + default: + case RADIANS: return this.normalize(degrees / 180.0f * Pif); + case DEGREES: return this.normalize(degrees); + } + } + + public double fromRadians(double radians) + { + switch (this) + { + default: + case RADIANS: return this.normalize(radians); + case DEGREES: return this.normalize(radians / Math.PI * 180.0); + } + } + + public float fromRadians(float radians) + { + switch (this) + { + default: + case RADIANS: return this.normalize(radians); + case DEGREES: return this.normalize(radians / Pif * 180.0f); + } + } + + public double fromUnit(AngleUnit them, double theirs) + { + switch (them) + { + default: + case RADIANS: return this.fromRadians(theirs); + case DEGREES: return this.fromDegrees(theirs); + } + } + + public float fromUnit(AngleUnit them, float theirs) + { + switch (them) + { + default: + case RADIANS: return this.fromRadians(theirs); + case DEGREES: return this.fromDegrees(theirs); + } + } + + //---------------------------------------------------------------------------------------------- + // Derived operations + //---------------------------------------------------------------------------------------------- + + public double toDegrees(double inOurUnits) + { + switch (this) + { + default: + case RADIANS: return DEGREES.fromRadians(inOurUnits); + case DEGREES: return DEGREES.fromDegrees(inOurUnits); + } + } + + public float toDegrees(float inOurUnits) + { + switch (this) + { + default: + case RADIANS: return DEGREES.fromRadians(inOurUnits); + case DEGREES: return DEGREES.fromDegrees(inOurUnits); + } + } + + public double toRadians(double inOurUnits) + { + switch (this) + { + default: + case RADIANS: return RADIANS.fromRadians(inOurUnits); + case DEGREES: return RADIANS.fromDegrees(inOurUnits); + } + } + + public float toRadians(float inOurUnits) + { + switch (this) + { + default: + case RADIANS: return RADIANS.fromRadians(inOurUnits); + case DEGREES: return RADIANS.fromDegrees(inOurUnits); + } + } + + //---------------------------------------------------------------------------------------------- + // Normalization + //---------------------------------------------------------------------------------------------- + + public double normalize(double mine) + { + switch (this) + { + default: + case RADIANS: return normalizeRadians(mine); + case DEGREES: return normalizeDegrees(mine); + } + } + + public float normalize(float mine) + { + switch (this) + { + default: + case RADIANS: return normalizeRadians(mine); + case DEGREES: return normalizeDegrees(mine); + } + } + + public static double normalizeDegrees(double degrees) + { + while (degrees >= 180.0) degrees -= 360.0; + while (degrees < -180.0) degrees += 360.0; + return degrees; + } + + public static float normalizeDegrees(float degrees) + { + return (float)normalizeDegrees((double)degrees); + } + + public static double normalizeRadians(double radians) + { + while (radians >= Math.PI) radians -= TwoPi; + while (radians < -Math.PI) radians += TwoPi; + return radians; + } + + public static float normalizeRadians(float radians) + { + return (float)normalizeRadians((double)radians); + } + + public UnnormalizedAngleUnit getUnnormalized() + { + switch (this) + { + default: + case RADIANS: return UnnormalizedAngleUnit.RADIANS; + case DEGREES: return UnnormalizedAngleUnit.DEGREES; + } + } + +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AxesOrder.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AxesOrder.java new file mode 100644 index 00000000..4bb3f743 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AxesOrder.java @@ -0,0 +1,103 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +/** + * {@link AxesOrder} indicates the chronological order of axes about which the three rotations + * of an {@link Orientation} take place. The geometry of three space is such that there are + * exactly twelve distinct rotational orders. + * + * @see Orientation + * @see AxesReference + * @see Euler Angles + */ +public enum AxesOrder +{ + XZX(new int[]{0, 2, 0}), XYX(new int[]{0, 1, 0}), YXY(new int[]{1, 0, 1}), + YZY(new int[]{1, 2, 1}), ZYZ(new int[]{2, 1, 2}), ZXZ(new int[]{2, 0, 2}), + XZY(new int[]{0, 2, 1}), XYZ(new int[]{0, 1, 2}), YXZ(new int[]{1, 0, 2}), + YZX(new int[]{1, 2, 0}), ZYX(new int[]{2, 1, 0}), ZXY(new int[]{2, 0, 1}); + + private final int[] indices; + + AxesOrder(int[] indices) + { + this.indices = indices; + } + + /** + * Returns the numerical axes indices associated with this {@link AxesOrder}. + * @return the numerical axes indices associated with this {@link AxesOrder}. + */ + public int[] indices() + { + return this.indices; + } + + /** + * Returns the {@link Axis axes} associated with this {@link AxesOrder}. + * @return the {@link Axis axes} associated with this {@link AxesOrder}. + */ + public Axis[] axes() + { + Axis[] result = new Axis[3]; + result[0] = Axis.fromIndex(this.indices[0]); + result[1] = Axis.fromIndex(this.indices[1]); + result[2] = Axis.fromIndex(this.indices[2]); + return result; + } + + /** + * Returns the {@link AxesOrder} which is the chronological reverse of the receiver. + * @return the {@link AxesOrder} which is the chronological reverse of the receiver. + */ + public AxesOrder reverse() + { + switch (this) + { + default: + case XZX: return XZX; + case XYX: return XYX; + case YXY: return YXY; + case YZY: return YZY; + case ZYZ: return ZYZ; + case ZXZ: return ZXZ; + case XZY: return YZX; + case XYZ: return ZYX; + case YXZ: return ZXY; + case YZX: return XZY; + case ZYX: return XYZ; + case ZXY: return YXZ; + } + } +} \ No newline at end of file diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AxesReference.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AxesReference.java new file mode 100644 index 00000000..bc34ce26 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AxesReference.java @@ -0,0 +1,57 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +/** + * {@link AxesReference} indicates whether we have intrinsic rotations, where the axes + * move with the object that is rotating, or extrinsic rotations, where they remain fixed + * in the world around the object. + * + * @see Orientation + * @see AxesOrder + * @see Euler Angles + */ +public enum AxesReference +{ + EXTRINSIC, INTRINSIC; + + public AxesReference reverse() + { + switch (this) + { + default: + case EXTRINSIC: return INTRINSIC; + case INTRINSIC: return EXTRINSIC; + } + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Axis.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Axis.java new file mode 100644 index 00000000..a8064ecb --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Axis.java @@ -0,0 +1,59 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +/** + * {@link Axis} enumerates the common X,Y,Z three-dimensional orthogonal axes. + */ +public enum Axis +{ + X(0), + Y(1), + Z(2), + UNKNOWN(-1); + + public int index; + + Axis(int index) { this.index = index; } + + public static Axis fromIndex(int index) + { + switch (index) + { + case 0: return X; + case 1: return Y; + case 2: return Z; + default: return UNKNOWN; + } + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/DistanceUnit.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/DistanceUnit.java new file mode 100644 index 00000000..e44675c3 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/DistanceUnit.java @@ -0,0 +1,202 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +import java.util.Locale; + +/** + * {@link DistanceUnit} represents a unit of measure of distance. + */ +public enum DistanceUnit +{ + METER(0), CM(1), MM(2), INCH(3); + public final byte bVal; + + public static final double infinity = Double.MAX_VALUE; + public static final double mmPerInch = 25.4; + public static final double mPerInch = mmPerInch * 0.001; + + DistanceUnit(int i) + { + this.bVal = (byte)i; + } + + //---------------------------------------------------------------------------------------------- + // Primitive operations + //---------------------------------------------------------------------------------------------- + + public double fromMeters(double meters) + { + if (meters==infinity) return infinity; + switch (this) + { + default: + case METER: return meters; + case CM: return meters * 100; + case MM: return meters * 1000; + case INCH: return meters / mPerInch; + } + } + + public double fromInches(double inches) + { + if (inches==infinity) return infinity; + switch (this) + { + default: + case METER: return inches * mPerInch; + case CM: return inches * mPerInch * 100; + case MM: return inches * mPerInch * 1000; + case INCH: return inches; + } + } + + public double fromCm(double cm) + { + if (cm==infinity) return infinity; + switch (this) + { + default: + case METER: return cm / 100; + case CM: return cm; + case MM: return cm * 10; + case INCH: return fromMeters(METER.fromCm(cm)); + } + } + + public double fromMm(double mm) + { + if (mm==infinity) return infinity; + switch (this) + { + default: + case METER: return mm / 1000; + case CM: return mm / 10; + case MM: return mm; + case INCH: return fromMeters(METER.fromMm(mm)); + } + } + + public double fromUnit(DistanceUnit him, double his) + { + switch (him) + { + default: + case METER: return this.fromMeters(his); + case CM: return this.fromCm(his); + case MM: return this.fromMm(his); + case INCH: return this.fromInches(his); + } + } + + //---------------------------------------------------------------------------------------------- + // Derived operations + //---------------------------------------------------------------------------------------------- + + public double toMeters(double inOurUnits) + { + switch (this) + { + default: + case METER: return METER.fromMeters(inOurUnits); + case CM: return METER.fromCm(inOurUnits); + case MM: return METER.fromMm(inOurUnits); + case INCH: return METER.fromInches(inOurUnits); + } + } + + public double toInches(double inOurUnits) + { + switch (this) + { + default: + case METER: return INCH.fromMeters(inOurUnits); + case CM: return INCH.fromCm(inOurUnits); + case MM: return INCH.fromMm(inOurUnits); + case INCH: return INCH.fromInches(inOurUnits); + } + } + + public double toCm(double inOurUnits) + { + switch (this) + { + default: + case METER: return CM.fromMeters(inOurUnits); + case CM: return CM.fromCm(inOurUnits); + case MM: return CM.fromMm(inOurUnits); + case INCH: return CM.fromInches(inOurUnits); + } + } + + public double toMm(double inOurUnits) + { + switch (this) + { + default: + case METER: return MM.fromMeters(inOurUnits); + case CM: return MM.fromCm(inOurUnits); + case MM: return MM.fromMm(inOurUnits); + case INCH: return MM.fromInches(inOurUnits); + } + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + public String toString(double inOurUnits) + { + switch (this) + { + default: + case METER: return String.format(Locale.getDefault(), "%.3fm", inOurUnits); + case CM: return String.format(Locale.getDefault(), "%.1fcm", inOurUnits); + case MM: return String.format(Locale.getDefault(), "%.0fmm", inOurUnits); + case INCH: return String.format(Locale.getDefault(), "%.2fin", inOurUnits); + } + } + + @Override public String toString() + { + switch (this) + { + default: + case METER: return "m"; + case CM: return "cm"; + case MM: return "mm"; + case INCH: return "in"; + } + } +} + diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java new file mode 100644 index 00000000..c5899e5b --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java @@ -0,0 +1,887 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.internal.system.Assert; + +import static java.lang.Math.sin; +import static java.lang.Math.cos; +import static java.lang.Math.asin; +import static java.lang.Math.acos; +import static java.lang.Math.atan2; +import static java.lang.Math.PI; + +/** + * Instances of {@link Orientation} represent a rotated stance in three-dimensional space + * by way of a set of three successive rotations. + * + *

There are several ways that a particular orientation in three-space can be represented. + * One way is by specifying a (unit) directional vector about which the orientation is to occur, + * together with a rotation angle about that axis. This representation is unique up to the sign of the + * direction and angle; that is a rotation {@code a} about a vector {@code v} produces the same + * rotation as a rotation {@code -a} about the vector {@code -v}. While this manner of specifying a + * rotation is easy to visualize if the vector in question is one of the cardinal axes (ie: X,Y, or Z), + * many find it more difficult to visualize more complex rotations in this manner.

+ * + *

An alternative, more common, way to represent a particular orientation in three-space is by means + * of indicating three angles of rotation about three successive axes. You might for example be familiar + * with the notions of heading, elevation, and bank angles for aircraft. Unfortunately, there are 24 + * different yet equivalent ways that a set of three rotational angles about three axes can represent + * the same effective rotation. As might be expected, this can be the source of much confusion. The + * 24 different representations break down as follows.

+ * + *

First is the matter of the axes reference: is the coordinate system in which the referred-to rotational + * axes reside a coordinate system that moves with (and so remains fixed relative to) the object being rotated, + * or do the axes remain fixed relative to the world around the object and are unaffected by the + * object's rotational motion? The former situation is referred to as an {@link AxesReference#INTRINSIC intrinsic} + * reference perspective while the latter is an {@link AxesReference#EXTRINSIC extrinsic} perspective. + * Both points of view are equally valid methodologies, but one or the other may be more understandable + * or useful in a given application situation. + *

+ * + *

The extrinsic-vs-intrinsic difference accounts for a factor of two in our list of 24 different + * representations. The remaining factor of 12 breaks down into whether the three rotations all use + * different axes (and so are a permutation of X, Y, and Z, of which there are six in number), or whether + * the first and last axes are the same and the middle one different (e.g. Z-Y-Z); this has three + * choices for the first axis (which is also used for the last) and two remaining choices for the + * second axis, for a total, again, of six possibilities. The geometry of three-space is such that these + * twelve choices are the only distinct representational possibilities. As with the extrinsic-vs- + * intrinsic difference, all twelve of these axis {@link AxesOrder order}s are equally valid ways of + * indicating orientation, but in any given application, one way may be more useful or easier to + * understand than another. + *

+ * + *

Even on top of all that, for a given intrinsic-vs-extrinsic distinction, and a given axes + * ordering, there are two sets of angle rotation that will produce the same orientation. For example, + * an extrinsic, XZX rotation of (in degrees) 90, -90, 0 is equivalent to an extrinsic, XZX rotation + * of -90, 90, -180.

+ * + *

As was mentioned, much confusion has historically arisen from talking about an orientation as + * a set of three angles without also clearly indicating which of the 24 representational possibilities + * one is working within. One aim of {@link Orientation} is to reduce that confusion by being explicitly + * clear about this issue: an {@link Orientation} always carries along with it the indication of the + * {@link AxesReference} and {@link AxesOrder} of the orientation. Methods are provided for converting + * an {@link Orientation} to and from its associated rotation matrix.

+ * + * @see Euler Angles + * @see Axis-Angle Representation + * @see Axes Conventions + * @see Rotation Matrix + */ +public class Orientation +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + /** + * whether we have extrinsic or intrinsic rotations + * + * @see #axesOrder + */ + public AxesReference axesReference; + + /** + * the order of axes around which our three rotations occur + * + * @see #axesReference + */ + public AxesOrder axesOrder; + + /** + * the unit in which the angles are expressed + */ + public AngleUnit angleUnit; + + /** + * the chronologically first rotation made in the {@link AxesOrder} + */ + public float firstAngle; + /** + * the chronologically second rotation made in the {@link AxesOrder} + */ + public float secondAngle; + /** + * the chronologically third rotation made in the {@link AxesOrder} + */ + public float thirdAngle; + + /** + * the time on the System.nanoTime() clock at which the data was acquired. If no + * timestamp is associated with this particular set of data, this value is zero. + */ + public long acquisitionTime; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public Orientation() + { + this(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.RADIANS, 0, 0, 0, 0); + } + + public Orientation(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float firstAngle, float secondAngle, float thirdAngle, long acquisitionTime) + { + this.axesReference = axesReference; + this.axesOrder = axesOrder; + this.angleUnit = angleUnit; + this.firstAngle = firstAngle; + this.secondAngle = secondAngle; + this.thirdAngle = thirdAngle; + this.acquisitionTime = acquisitionTime; + } + + /** + * Converts this {@link Orientation} to one with the indicated angular units. + * + * @param angleUnit the units to use in the returned [@link Orientation} + * @return a new [@link Orientation} with the same data but in the indicated units + */ + public Orientation toAngleUnit(AngleUnit angleUnit) + { + if (angleUnit != this.angleUnit) + { + return new Orientation(this.axesReference, this.axesOrder, angleUnit, + angleUnit.fromUnit(this.angleUnit, firstAngle), + angleUnit.fromUnit(this.angleUnit, secondAngle), + angleUnit.fromUnit(this.angleUnit, thirdAngle), + this.acquisitionTime); + } + else + return this; + } + + /** + * Converts the {@link Orientation} to an equivalent one with the indicted point of view. + * + * @param axesReference whether we wish to consider rotations from an extrinsic or intrinsic point of view + * @return an equivalent orientation but with the indicated point of view. + */ + public Orientation toAxesReference(AxesReference axesReference) + { + if (this.axesReference != axesReference) + { + /** + * Theorem: Any extrinsic rotation is equivalent to an intrinsic rotation by + * the same angles but with inverted order of elemental orientations, and vice versa. + * @see Euler Angles + */ + Assert.assertTrue(axesReference == this.axesReference.reverse()); + return new Orientation(this.axesReference.reverse(), this.axesOrder.reverse(), this.angleUnit, + this.thirdAngle, this.secondAngle, this.firstAngle, this.acquisitionTime); + } + else + return this; + } + + /** + * Converts the {@link Orientation} to an equivalent one with the indicated ordering of axes + * @param axesOrder the desired ordering of axes + * @return an equivalent orientation with the indicated axes order + */ + public Orientation toAxesOrder(AxesOrder axesOrder) + { + if (this.axesOrder != axesOrder) + { + return Orientation.getOrientation(this.getRotationMatrix(), this.axesReference, axesOrder, this.angleUnit); + } + else + return this; + } + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + @Override public String toString() + { + if (this.angleUnit == AngleUnit.DEGREES) + return String.format("{%s %s %.0f %.0f %.0f}", this.axesReference.toString(), this.axesOrder.toString(), this.firstAngle, this.secondAngle, this.thirdAngle); + else + return String.format("{%s %s %.3f %.3f %.3f}", this.axesReference.toString(), this.axesOrder.toString(), this.firstAngle, this.secondAngle, this.thirdAngle); + } + + //---------------------------------------------------------------------------------------------- + // Rotation Matrices + //---------------------------------------------------------------------------------------------- + + /** + * Returns the rotation matrix associated with the receiver {@link Orientation}. + * + * @return the rotation matrix associated with the receiver {@link Orientation}. + * @see #getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float) + * @see Rotation Matrix + */ + public OpenGLMatrix getRotationMatrix() + { + return getRotationMatrix(this.axesReference, this.axesOrder, this.angleUnit, this.firstAngle, this.secondAngle, this.thirdAngle); + } + + /** + * Returns the rotation matrix associated with a particular set of three rotational angles. + * + * @return the rotation matrix associated with a particular set of three rotational angles. + * @see #getRotationMatrix() + * @see Rotation Matrix + */ + public static OpenGLMatrix getRotationMatrix(AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit, float firstAngle, float secondAngle, float thirdAngle) + { + if (axesReference == AxesReference.INTRINSIC) + { + /** + * Theorem: Any extrinsic rotation is equivalent to an intrinsic rotation by the same + * angles but with inverted order of elemental orientations, and vice versa. + * @see Euler Angles + */ + return getRotationMatrix(axesReference.reverse(), axesOrder.reverse(), unit, thirdAngle, secondAngle, firstAngle); + } + + /** + * The extrinsic case takes some work. + * + * Implementation note: these computations were created automatically from symbolic Mathematica expressions. + * Each computes the intrinsic rotation matrix of a given {@link AxesOrder}, where the axes in the {@link AxesOrder} + * are used left to right chronologically and the angles are applied chronologically as well. + * + * For example, the entry for YXZ is rotation matrix of the extrinsic rotation which rotates first + * Y(firstAngle), then X(secondAngle), and finally Z(thirdAngle). The rotation matrix in this case is + * Z(thirdAngle).X(secondAngle).Y(firstAngle) + * as the matrix order (when *post*-multiplying vectors, as is always done in modern computer graphics systems), + * which you will notice has the matrices in reverse order from the chronological sequence of extrinsic rotations. + */ + + firstAngle = unit.toRadians(firstAngle); + secondAngle = unit.toRadians(secondAngle); + thirdAngle = unit.toRadians(thirdAngle); + + float m00, m01, m02; + float m10, m11, m12; + float m20, m21, m22; + + switch (axesOrder) + { + default: + case XZX: + m00 = ((float) (cos(secondAngle))); + m01 = ((float) (-(cos(firstAngle) * sin(secondAngle)))); + m02 = ((float) (sin(firstAngle) * sin(secondAngle))); + m10 = ((float) (cos(thirdAngle) * sin(secondAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(firstAngle) * sin(thirdAngle)) - cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m20 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m21 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * cos(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + break; + case XYX: + m00 = ((float) (cos(secondAngle))); + m01 = ((float) (sin(firstAngle) * sin(secondAngle))); + m02 = ((float) (cos(firstAngle) * sin(secondAngle))); + m10 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(firstAngle) * cos(secondAngle) * sin(thirdAngle)) - cos(thirdAngle) * sin(firstAngle))); + m20 = ((float) (-(cos(thirdAngle) * sin(secondAngle)))); + m21 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + break; + case YXY: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m02 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * cos(secondAngle) * sin(thirdAngle))); + m10 = ((float) (sin(firstAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle))); + m12 = ((float) (-(cos(firstAngle) * sin(secondAngle)))); + m20 = ((float) (-(cos(firstAngle) * sin(thirdAngle)) - cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m21 = ((float) (cos(thirdAngle) * sin(secondAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + break; + case YZY: + m00 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(thirdAngle) * sin(secondAngle)))); + m02 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m10 = ((float) (cos(firstAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle))); + m12 = ((float) (sin(firstAngle) * sin(secondAngle))); + m20 = ((float) (-(cos(firstAngle) * cos(secondAngle) * sin(thirdAngle)) - cos(thirdAngle) * sin(firstAngle))); + m21 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + break; + case ZYZ: + m00 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(firstAngle) * sin(thirdAngle)) - cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m02 = ((float) (cos(thirdAngle) * sin(secondAngle))); + m10 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * cos(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m20 = ((float) (-(cos(firstAngle) * sin(secondAngle)))); + m21 = ((float) (sin(firstAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle))); + break; + case ZXZ: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(firstAngle) * cos(secondAngle) * sin(thirdAngle)) - cos(thirdAngle) * sin(firstAngle))); + m02 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m10 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(thirdAngle) * sin(secondAngle)))); + m20 = ((float) (sin(firstAngle) * sin(secondAngle))); + m21 = ((float) (cos(firstAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle))); + break; + case XZY: + m00 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m01 = ((float) (sin(firstAngle) * sin(thirdAngle) - cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m02 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(thirdAngle) * sin(firstAngle) * sin(secondAngle))); + m10 = ((float) (sin(secondAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle))); + m12 = ((float) (-(cos(secondAngle) * sin(firstAngle)))); + m20 = ((float) (-(cos(secondAngle) * sin(thirdAngle)))); + m21 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) - sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + break; + case XYZ: + m00 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m01 = ((float) (cos(thirdAngle) * sin(firstAngle) * sin(secondAngle) - cos(firstAngle) * sin(thirdAngle))); + m02 = ((float) (sin(firstAngle) * sin(thirdAngle) + cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m10 = ((float) (cos(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) + sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m12 = ((float) (cos(firstAngle) * sin(secondAngle) * sin(thirdAngle) - cos(thirdAngle) * sin(firstAngle))); + m20 = ((float) (-sin(secondAngle))); + m21 = ((float) (cos(secondAngle) * sin(firstAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle))); + break; + case YXZ: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) - sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(secondAngle) * sin(thirdAngle)))); + m02 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m10 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(thirdAngle) * sin(firstAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m12 = ((float) (sin(firstAngle) * sin(thirdAngle) - cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m20 = ((float) (-(cos(secondAngle) * sin(firstAngle)))); + m21 = ((float) (sin(secondAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle))); + break; + case YZX: + m00 = ((float) (cos(firstAngle) * cos(secondAngle))); + m01 = ((float) (-sin(secondAngle))); + m02 = ((float) (cos(secondAngle) * sin(firstAngle))); + m10 = ((float) (sin(firstAngle) * sin(thirdAngle) + cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m12 = ((float) (cos(thirdAngle) * sin(firstAngle) * sin(secondAngle) - cos(firstAngle) * sin(thirdAngle))); + m20 = ((float) (cos(firstAngle) * sin(secondAngle) * sin(thirdAngle) - cos(thirdAngle) * sin(firstAngle))); + m21 = ((float) (cos(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) + sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + break; + case ZYX: + m00 = ((float) (cos(firstAngle) * cos(secondAngle))); + m01 = ((float) (-(cos(secondAngle) * sin(firstAngle)))); + m02 = ((float) (sin(secondAngle))); + m10 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) - sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(secondAngle) * sin(thirdAngle)))); + m20 = ((float) (sin(firstAngle) * sin(thirdAngle) - cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m21 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(thirdAngle) * sin(firstAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle) * cos(thirdAngle))); + break; + case ZXY: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) + sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m01 = ((float) (cos(firstAngle) * sin(secondAngle) * sin(thirdAngle) - cos(thirdAngle) * sin(firstAngle))); + m02 = ((float) (cos(secondAngle) * sin(thirdAngle))); + m10 = ((float) (cos(secondAngle) * sin(firstAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle))); + m12 = ((float) (-sin(secondAngle))); + m20 = ((float) (cos(thirdAngle) * sin(firstAngle) * sin(secondAngle) - cos(firstAngle) * sin(thirdAngle))); + m21 = ((float) (sin(firstAngle) * sin(thirdAngle) + cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle) * cos(thirdAngle))); + break; + } + + OpenGLMatrix result = new OpenGLMatrix(); + result.put(0, 0, m00); + result.put(0, 1, m01); + result.put(0, 2, m02); + result.put(1, 0, m10); + result.put(1, 1, m11); + result.put(1, 2, m12); + result.put(2, 0, m20); + result.put(2, 1, m21); + result.put(2, 2, m22); + return result; + } + + /** + * Given a rotation matrix, and an {@link AxesReference} and {@link AxesOrder}, returns an orientation + * that would produce that rotation matrix. + * + * @param rot the matrix whose orientation is to be determined + * @param axesReference whether wish an extrinsic or intrinsic reference for the axes + * @param axesOrder the order in which the axes are to be rotated + * @param unit the angle units in which the orientation is to be returned + * @return an orientation that will produce the given rotation matrix + * @see Orientation + * @see #getOrientation(MatrixF, AxesReference, AxesOrder, AngleUnit, AngleSet) + * @see Rotation Matrix + */ + public static Orientation getOrientation(MatrixF rot, AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit) + { + /** + * Run both choices and return the one that uses smaller sets of angles. This is just a heuristic + * to choose which angle set is the most aesthetically pleasing. Both angle sets are equally valid. + */ + Orientation one = getOrientation(rot, axesReference, axesOrder, unit, AngleSet.THEONE); + Orientation theOther = getOrientation(rot, axesReference, axesOrder, unit, AngleSet.THEOTHER); + + VectorF vOne = new VectorF(one.firstAngle, one.secondAngle, one.thirdAngle); + VectorF vOther = new VectorF(theOther.firstAngle, theOther.secondAngle, theOther.thirdAngle); + + return vOne.magnitude() <= vOther.magnitude() ? one : theOther; + } + + /** + * {@link AngleSet} is used to distinguish between the two sets of angles that will produce + * a given rotation in a given axes reference and a given axes order + */ + public enum AngleSet { THEONE, THEOTHER }; + + /** + * Given a rotation matrix, and an {@link AxesReference} and {@link AxesOrder}, returns an orientation + * that would produce that rotation matrix. + * + * @param rot the matrix whose orientation is to be determined + * @param axesReference whether wish an extrinsic or intrinsic reference for the axes + * @param axesOrder the order in which the axes are to be rotated + * @param unit the angle units in which the orientation is to be returned + * @param angleSet which of the two sets angles which can produce the rotation matrix is desired + * @return an orientation that will produce the given rotation matrix + * @see #getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float) + * @see Rotation Matrix + */ + public static Orientation getOrientation(MatrixF rot, AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit, AngleSet angleSet) + { + float firstAngle, secondAngle, thirdAngle; + + if (axesReference == AxesReference.INTRINSIC) + { + return getOrientation(rot, axesReference.reverse(), axesOrder.reverse(), unit, angleSet).toAxesReference(axesReference); + } + + /** + * The extrinsic case takes some work. + * + * Implementation note: these computations contained in this 'switch' were derived automatically + * from symbolic Mathematica representations of the corresponding twelve forms of rotation + * matrix. The output of that automatic processing was literally copied and pasted from Mathematica + * into this Java source without intervention of human editing (save for reformatting of the code) + * thus significantly reducing the chance that errors might be inadvertently introduced. + * + * The cases labelled here as "arbitrary" are situations in which + * gimbal lock occurs. In those situations, + * the three angles are not uniquely specified by the rotation. Instead, only the sum or difference + * (as the case may be) of two of the axes is determined. In those situations, we make an + * arbitrary choice for the value of one of those two axes, then appropriately compute the other. + */ + float test; + switch (axesOrder) + { + default: + case XZX: + test = rot.get(0, 0); /* cos(secondAngle) */ + if (test == 1) + { + secondAngle = (float) 0; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(2, 1) == sin(firstAngle + thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(2, 1), rot.get(1, 1)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) PI; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 2) == sin(firstAngle - thirdAngle) */ + /* rot.get(2, 2) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(1, 2), rot.get(2, 2))); + } + else + { + /* rot.get(0, 0) == cos(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(0, 0)) : -acos(rot.get(0, 0))); + /* rot.get(0, 2) == sin(firstAngle) * sin(secondAngle) */ + /* rot.get(0, 1) == -(cos(firstAngle) * sin(secondAngle)) */ + firstAngle = (float) atan2(rot.get(0, 2) / sin(secondAngle), -rot.get(0, 1) / sin(secondAngle)); + /* rot.get(2, 0) == sin(secondAngle) * sin(thirdAngle) */ + /* rot.get(1, 0) == cos(thirdAngle) * sin(secondAngle) */ + thirdAngle = (float) atan2(rot.get(2, 0) / sin(secondAngle), rot.get(1, 0) / sin(secondAngle)); + } + break; + case XYX: + test = rot.get(0, 0); /* cos(secondAngle) */ + if (test == 1) + { + secondAngle = (float) 0; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(2, 1) == sin(firstAngle + thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(2, 1), rot.get(1, 1)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) PI; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 2) == -sin(firstAngle - thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(-rot.get(1, 2), rot.get(1, 1))); + } + else + { + /* rot.get(0, 0) == cos(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(0, 0)) : -acos(rot.get(0, 0))); + /* rot.get(0, 1) == sin(firstAngle) * sin(secondAngle) */ + /* rot.get(0, 2) == cos(firstAngle) * sin(secondAngle) */ + firstAngle = (float) atan2(rot.get(0, 1) / sin(secondAngle), rot.get(0, 2) / sin(secondAngle)); + /* rot.get(1, 0) == sin(secondAngle) * sin(thirdAngle) */ + /* rot.get(2, 0) == -(cos(thirdAngle) * sin(secondAngle)) */ + thirdAngle = (float) atan2(rot.get(1, 0) / sin(secondAngle), -rot.get(2, 0) / sin(secondAngle)); + } + break; + case YXY: + test = rot.get(1, 1); /* cos(secondAngle) */ + if (test == 1) + { + secondAngle = (float) 0; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(0, 0)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) PI; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == sin(firstAngle - thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(0, 2), rot.get(0, 0))); + } + else + { + /* rot.get(1, 1) == cos(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(1, 1)) : -acos(rot.get(1, 1))); + /* rot.get(1, 0) == sin(firstAngle) * sin(secondAngle) */ + /* rot.get(1, 2) == -(cos(firstAngle) * sin(secondAngle)) */ + firstAngle = (float) atan2(rot.get(1, 0) / sin(secondAngle), -rot.get(1, 2) / sin(secondAngle)); + /* rot.get(0, 1) == sin(secondAngle) * sin(thirdAngle) */ + /* rot.get(2, 1) == cos(thirdAngle) * sin(secondAngle) */ + thirdAngle = (float) atan2(rot.get(0, 1) / sin(secondAngle), rot.get(2, 1) / sin(secondAngle)); + } + break; + case YZY: + test = rot.get(1, 1); /* cos(secondAngle) */ + if (test == 1) + { + secondAngle = (float) 0; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(0, 0)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) PI; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == -sin(firstAngle - thirdAngle) */ + /* rot.get(2, 2) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(-rot.get(0, 2), rot.get(2, 2))); + } + else + { + /* rot.get(1, 1) == cos(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(1, 1)) : -acos(rot.get(1, 1))); + /* rot.get(1, 2) == sin(firstAngle) * sin(secondAngle) */ + /* rot.get(1, 0) == cos(firstAngle) * sin(secondAngle) */ + firstAngle = (float) atan2(rot.get(1, 2) / sin(secondAngle), rot.get(1, 0) / sin(secondAngle)); + /* rot.get(2, 1) == sin(secondAngle) * sin(thirdAngle) */ + /* rot.get(0, 1) == -(cos(thirdAngle) * sin(secondAngle)) */ + thirdAngle = (float) atan2(rot.get(2, 1) / sin(secondAngle), -rot.get(0, 1) / sin(secondAngle)); + } + break; + case ZYZ: + test = rot.get(2, 2); /* cos(secondAngle) */ + if (test == 1) + { + secondAngle = (float) 0; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 0) == sin(firstAngle + thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(1, 0), rot.get(0, 0)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) PI; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 1) == sin(firstAngle - thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(0, 1), rot.get(1, 1))); + } + else + { + /* rot.get(2, 2) == cos(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(2, 2)) : -acos(rot.get(2, 2))); + /* rot.get(2, 1) == sin(firstAngle) * sin(secondAngle) */ + /* rot.get(2, 0) == -(cos(firstAngle) * sin(secondAngle)) */ + firstAngle = (float) atan2(rot.get(2, 1) / sin(secondAngle), -rot.get(2, 0) / sin(secondAngle)); + /* rot.get(1, 2) == sin(secondAngle) * sin(thirdAngle) */ + /* rot.get(0, 2) == cos(thirdAngle) * sin(secondAngle) */ + thirdAngle = (float) atan2(rot.get(1, 2) / sin(secondAngle), rot.get(0, 2) / sin(secondAngle)); + } + break; + case ZXZ: + test = rot.get(2, 2); /* cos(secondAngle) */ + if (test == 1) + { + secondAngle = (float) 0; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 0) == sin(firstAngle + thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(1, 0), rot.get(0, 0)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) PI; + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 1) == -sin(firstAngle - thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(-rot.get(0, 1), rot.get(0, 0))); + } + else + { + /* rot.get(2, 2) == cos(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(2, 2)) : -acos(rot.get(2, 2))); + /* rot.get(2, 0) == sin(firstAngle) * sin(secondAngle) */ + /* rot.get(2, 1) == cos(firstAngle) * sin(secondAngle) */ + firstAngle = (float) atan2(rot.get(2, 0) / sin(secondAngle), rot.get(2, 1) / sin(secondAngle)); + /* rot.get(0, 2) == sin(secondAngle) * sin(thirdAngle) */ + /* rot.get(1, 2) == -(cos(thirdAngle) * sin(secondAngle)) */ + thirdAngle = (float) atan2(rot.get(0, 2) / sin(secondAngle), -rot.get(1, 2) / sin(secondAngle)); + } + break; + case XZY: + test = rot.get(1, 0); /* sin(secondAngle) */ + if (test == 1) + { + secondAngle = (float) (PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ + /* rot.get(2, 2) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(2, 2)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) (-PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(2, 1) == sin(firstAngle - thirdAngle) */ + /* rot.get(0, 1) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(2, 1), rot.get(0, 1))); + } + else + { + /* rot.get(1, 0) == sin(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? asin(rot.get(1, 0)) : PI - asin(rot.get(1, 0))); + /* rot.get(1, 2) == -(cos(secondAngle) * sin(firstAngle)) */ + /* rot.get(1, 1) == cos(firstAngle) * cos(secondAngle) */ + firstAngle = (float) atan2(-rot.get(1, 2) / cos(secondAngle), rot.get(1, 1) / cos(secondAngle)); + /* rot.get(2, 0) == -(cos(secondAngle) * sin(thirdAngle)) */ + /* rot.get(0, 0) == cos(secondAngle) * cos(thirdAngle) */ + thirdAngle = (float) atan2(-rot.get(2, 0) / cos(secondAngle), rot.get(0, 0) / cos(secondAngle)); + } + break; + case XYZ: + test = rot.get(2, 0); /* -sin(secondAngle) */ + if (test == -1) + { + secondAngle = (float) (PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 1) == sin(firstAngle - thirdAngle) */ + /* rot.get(0, 2) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(0, 1), rot.get(0, 2))); + } + else if (test == 1) + { + secondAngle = (float) (-PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 1) == -sin(firstAngle + thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(-rot.get(0, 1), rot.get(1, 1)) - firstAngle); + } + else + { + /* rot.get(2, 0) == -sin(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? -asin(rot.get(2, 0)) : PI + asin(rot.get(2, 0))); + /* rot.get(2, 1) == cos(secondAngle) * sin(firstAngle) */ + /* rot.get(2, 2) == cos(firstAngle) * cos(secondAngle) */ + firstAngle = (float) atan2(rot.get(2, 1) / cos(secondAngle), rot.get(2, 2) / cos(secondAngle)); + /* rot.get(1, 0) == cos(secondAngle) * sin(thirdAngle) */ + /* rot.get(0, 0) == cos(secondAngle) * cos(thirdAngle) */ + thirdAngle = (float) atan2(rot.get(1, 0) / cos(secondAngle), rot.get(0, 0) / cos(secondAngle)); + } + break; + case YXZ: + test = rot.get(2, 1); /* sin(secondAngle) */ + if (test == 1) + { + secondAngle = (float) (PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(0, 0)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) (-PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 2) == sin(firstAngle - thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(0, 2), rot.get(0, 0))); + } + else + { + /* rot.get(2, 1) == sin(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? asin(rot.get(2, 1)) : PI - asin(rot.get(2, 1))); + /* rot.get(2, 0) == -(cos(secondAngle) * sin(firstAngle)) */ + /* rot.get(2, 2) == cos(firstAngle) * cos(secondAngle) */ + firstAngle = (float) atan2(-rot.get(2, 0) / cos(secondAngle), rot.get(2, 2) / cos(secondAngle)); + /* rot.get(0, 1) == -(cos(secondAngle) * sin(thirdAngle)) */ + /* rot.get(1, 1) == cos(secondAngle) * cos(thirdAngle) */ + thirdAngle = (float) atan2(-rot.get(0, 1) / cos(secondAngle), rot.get(1, 1) / cos(secondAngle)); + } + break; + case YZX: + test = rot.get(0, 1); /* -sin(secondAngle) */ + if (test == -1) + { + secondAngle = (float) (PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 2) == sin(firstAngle - thirdAngle) */ + /* rot.get(1, 0) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(1, 2), rot.get(1, 0))); + } + else if (test == 1) + { + secondAngle = (float) (-PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 2) == -sin(firstAngle + thirdAngle) */ + /* rot.get(2, 2) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(-rot.get(1, 2), rot.get(2, 2)) - firstAngle); + } + else + { + /* rot.get(0, 1) == -sin(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? -asin(rot.get(0, 1)) : PI + asin(rot.get(0, 1))); + /* rot.get(0, 2) == cos(secondAngle) * sin(firstAngle) */ + /* rot.get(0, 0) == cos(firstAngle) * cos(secondAngle) */ + firstAngle = (float) atan2(rot.get(0, 2) / cos(secondAngle), rot.get(0, 0) / cos(secondAngle)); + /* rot.get(2, 1) == cos(secondAngle) * sin(thirdAngle) */ + /* rot.get(1, 1) == cos(secondAngle) * cos(thirdAngle) */ + thirdAngle = (float) atan2(rot.get(2, 1) / cos(secondAngle), rot.get(1, 1) / cos(secondAngle)); + } + break; + case ZYX: + test = rot.get(0, 2); /* sin(secondAngle) */ + if (test == 1) + { + secondAngle = (float) (PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 0) == sin(firstAngle + thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(rot.get(1, 0), rot.get(1, 1)) - firstAngle); + } + else if (test == -1) + { + secondAngle = (float) (-PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(1, 0) == sin(firstAngle - thirdAngle) */ + /* rot.get(1, 1) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(1, 0), rot.get(1, 1))); + } + else + { + /* rot.get(0, 2) == sin(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? asin(rot.get(0, 2)) : PI - asin(rot.get(0, 2))); + /* rot.get(0, 1) == -(cos(secondAngle) * sin(firstAngle)) */ + /* rot.get(0, 0) == cos(firstAngle) * cos(secondAngle) */ + firstAngle = (float) atan2(-rot.get(0, 1) / cos(secondAngle), rot.get(0, 0) / cos(secondAngle)); + /* rot.get(1, 2) == -(cos(secondAngle) * sin(thirdAngle)) */ + /* rot.get(2, 2) == cos(secondAngle) * cos(thirdAngle) */ + thirdAngle = (float) atan2(-rot.get(1, 2) / cos(secondAngle), rot.get(2, 2) / cos(secondAngle)); + } + break; + case ZXY: + test = rot.get(1, 2); /* -sin(secondAngle) */ + if (test == -1) + { + secondAngle = (float) (PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(2, 0) == sin(firstAngle - thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ + thirdAngle = (float) (firstAngle - atan2(rot.get(2, 0), rot.get(0, 0))); + } + else if (test == 1) + { + secondAngle = (float) (-PI / 2); + firstAngle = (float) 0; /* arbitrary */ + /* rot.get(0, 1) == -sin(firstAngle + thirdAngle) */ + /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ + thirdAngle = (float) (atan2(-rot.get(0, 1), rot.get(0, 0)) - firstAngle); + } + else + { + /* rot.get(1, 2) == -sin(secondAngle) */ + secondAngle = (float) (angleSet == AngleSet.THEONE ? -asin(rot.get(1, 2)) : PI + asin(rot.get(1, 2))); + /* rot.get(1, 0) == cos(secondAngle) * sin(firstAngle) */ + /* rot.get(1, 1) == cos(firstAngle) * cos(secondAngle) */ + firstAngle = (float) atan2(rot.get(1, 0) / cos(secondAngle), rot.get(1, 1) / cos(secondAngle)); + /* rot.get(0, 2) == cos(secondAngle) * sin(thirdAngle) */ + /* rot.get(2, 2) == cos(secondAngle) * cos(thirdAngle) */ + thirdAngle = (float) atan2(rot.get(0, 2) / cos(secondAngle), rot.get(2, 2) / cos(secondAngle)); + } + break; + } + + return new Orientation(axesReference, axesOrder, unit, + unit.fromRadians(firstAngle), unit.fromRadians(secondAngle), unit.fromRadians(thirdAngle), + 0); + } + +} \ No newline at end of file diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java new file mode 100644 index 00000000..9e68bedd --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java @@ -0,0 +1,89 @@ +/* + * Copyright (c) 2024 Dryw Wade + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcore.external.navigation; + +import androidx.annotation.NonNull; + +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import java.util.Locale; + +/** + * Pose3D represents the position and orientation of an object in 3D space. + */ +public class Pose3D +{ + protected final Position position; + protected final YawPitchRollAngles orientation; + + public Pose3D(Position position, YawPitchRollAngles orientation) + { + this.position = position; + this.orientation = orientation; + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + @NonNull + @Override public String toString() + { + return String.format(Locale.getDefault(), + "position=%s, orientation=%s", + position.toString(), + orientation.toString()); + } + + /** + * A 3D orientation. + * + * The axis mapping is defined by the code that creates objects from this class. One should not assume that + * pitch, for example, is along the x axis. Consult the documentation of the API that returns + * a Pose3D for its axis mapping. + */ + public YawPitchRollAngles getOrientation() + { + return orientation; + } + + /** + * A 3D position. + */ + public Position getPosition() + { + return position; + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java new file mode 100644 index 00000000..6d69652a --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java @@ -0,0 +1,101 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +import java.util.Locale; + +/** + * Instances of {@link Position} represent a three-dimensional distance in a particular distance unit. + * + * @see Acceleration + * @see Position + */ +public class Position +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + public DistanceUnit unit; + + public double x; + public double y; + public double z; + + /** + * the time on the System.nanoTime() clock at which the data was acquired. If no + * timestamp is associated with this particular set of data, this value is zero. + */ + public long acquisitionTime; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public Position() + { + this(DistanceUnit.MM, 0, 0, 0, 0); + } + + public Position(DistanceUnit unit, double x, double y, double z, long acquisitionTime) + { + this.unit = unit; + this.x = x; + this.y = y; + this.z = z; + this.acquisitionTime = acquisitionTime; + } + + public Position toUnit(DistanceUnit distanceUnit) + { + if (distanceUnit != this.unit) + { + return new Position(distanceUnit, + distanceUnit.fromUnit(this.unit, x), + distanceUnit.fromUnit(this.unit, y), + distanceUnit.fromUnit(this.unit, z), + this.acquisitionTime); + } + else + return this; + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + @Override public String toString() + { + return String.format(Locale.getDefault(), "(%.3f %.3f %.3f)%s", x, y, z, unit.toString()); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java new file mode 100644 index 00000000..5679223c --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java @@ -0,0 +1,197 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +/** + * An {@link UnnormalizedAngleUnit} represents angles in different units of measure and + * provides utility methods to convert across units. {@link UnnormalizedAngleUnit} does not + * maintain angle information internally, but only helps organize + * and use angle measures that may be maintained separately across various contexts. + *

+ * Angles can be distinguished along (at least) two axes: + * Normalized angles are of most utility when dealing with physical angles, as normalization + * removes ambiguity of representation. In particular, two angles can be compared for equality + * by subtracting them, normalizing, and testing whether the absolute value of the result is + * smaller than some tolerance threshold. This approach neatly handles all cases of cyclical + * wrapping without unexpected discontinuities. + *

+ *

+ * Unnormalized angles can be handy when the angular quantity is not a physical angle but some + * related quantity such as an angular velocity or acceleration, where the + * quantity in question lacks the 360-degree cyclical equivalence of a physical angle. + *

+ *

+ * {@link AngleUnit} expresses normalized angles, while {@link UnnormalizedAngleUnit} expresses unnormalized ones + *

+ */ +@SuppressWarnings("WeakerAccess") +public enum UnnormalizedAngleUnit +{ + DEGREES(0), RADIANS(1); + public final byte bVal; + + UnnormalizedAngleUnit(int i) + { + bVal = (byte) i; + } + + //---------------------------------------------------------------------------------------------- + // Primitive operations + //---------------------------------------------------------------------------------------------- + + public double fromDegrees(double degrees) + { + switch (this) + { + default: + case RADIANS: return (degrees / 180.0 * Math.PI); + case DEGREES: return (degrees); + } + } + + public float fromDegrees(float degrees) + { + switch (this) + { + default: + case RADIANS: return (degrees / 180.0f * AngleUnit.Pif); + case DEGREES: return (degrees); + } + } + + public double fromRadians(double radians) + { + switch (this) + { + default: + case RADIANS: return (radians); + case DEGREES: return (radians / Math.PI * 180.0); + } + } + + public float fromRadians(float radians) + { + switch (this) + { + default: + case RADIANS: return (radians); + case DEGREES: return (radians / AngleUnit.Pif * 180.0f); + } + } + + public double fromUnit(UnnormalizedAngleUnit them, double theirs) + { + switch (them) + { + default: + case RADIANS: return this.fromRadians(theirs); + case DEGREES: return this.fromDegrees(theirs); + } + } + + public float fromUnit(UnnormalizedAngleUnit them, float theirs) + { + switch (them) + { + default: + case RADIANS: return this.fromRadians(theirs); + case DEGREES: return this.fromDegrees(theirs); + } + } + + public double fromUnit(AngleUnit them, double theirs) { + return this.fromUnit(them.getUnnormalized(), theirs); + } + + public float fromUnit(AngleUnit them, float theirs) { + return this.fromUnit(them.getUnnormalized(), theirs); + } + + //---------------------------------------------------------------------------------------------- + // Derived operations + //---------------------------------------------------------------------------------------------- + + public double toDegrees(double inOurUnits) + { + switch (this) + { + default: + case RADIANS: return DEGREES.fromRadians(inOurUnits); + case DEGREES: return DEGREES.fromDegrees(inOurUnits); + } + } + + public float toDegrees(float inOurUnits) + { + switch (this) + { + default: + case RADIANS: return DEGREES.fromRadians(inOurUnits); + case DEGREES: return DEGREES.fromDegrees(inOurUnits); + } + } + + public double toRadians(double inOurUnits) + { + switch (this) + { + default: + case RADIANS: return RADIANS.fromRadians(inOurUnits); + case DEGREES: return RADIANS.fromDegrees(inOurUnits); + } + } + + public float toRadians(float inOurUnits) + { + switch (this) + { + default: + case RADIANS: return RADIANS.fromRadians(inOurUnits); + case DEGREES: return RADIANS.fromDegrees(inOurUnits); + } + } + + //---------------------------------------------------------------------------------------------- + // Normalization + //---------------------------------------------------------------------------------------------- + + public AngleUnit getNormalized() + { + switch (this) + { + default: + case RADIANS: return AngleUnit.RADIANS; + case DEGREES: return AngleUnit.DEGREES; + } + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Velocity.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Velocity.java new file mode 100644 index 00000000..e8b0009b --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Velocity.java @@ -0,0 +1,104 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +import java.util.Locale; + +/** + * Instances of {@link Velocity} represent the derivative of {@link Position} over time. + * + * @see Position + * @see Acceleration + */ +public class Velocity +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + /** + * The distance units in which this velocity is expressed. The time unit is always "per second". + */ + public DistanceUnit unit; + + public double xVeloc; + public double yVeloc; + public double zVeloc; + + /** + * the time on the System.nanoTime() clock at which the data was acquired. If no + * timestamp is associated with this particular set of data, this value is zero. + */ + public long acquisitionTime; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public Velocity() + { + this(DistanceUnit.MM, 0, 0, 0, 0); + } + + public Velocity(DistanceUnit unit, double xVeloc, double yVeloc, double zVeloc, long acquisitionTime) + { + this.unit = unit; + this.xVeloc = xVeloc; + this.yVeloc = yVeloc; + this.zVeloc = zVeloc; + this.acquisitionTime = acquisitionTime; + } + + public Velocity toUnit(DistanceUnit distanceUnit) + { + if (distanceUnit != this.unit) + { + return new Velocity(distanceUnit, + distanceUnit.fromUnit(this.unit, xVeloc), + distanceUnit.fromUnit(this.unit, yVeloc), + distanceUnit.fromUnit(this.unit, zVeloc), + this.acquisitionTime); + } + else + return this; + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + @Override public String toString() + { + return String.format(Locale.getDefault(), "(%.3f %.3f %.3f)%s/s", xVeloc, yVeloc, zVeloc, unit.toString()); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/YawPitchRollAngles.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/YawPitchRollAngles.java new file mode 100644 index 00000000..328362fa --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/YawPitchRollAngles.java @@ -0,0 +1,144 @@ +/* +Copyright (c) 2022 REV Robotics + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of REV Robotics nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.navigation; + +import java.util.Locale; + +/** + * A simplified view of the orientation of an object in 3D space. + *

+ * Yaw is side-to-side lateral rotation, where the object remains flat, but turns left and right. + * Sometimes yaw is also referred to as "heading". + *

+ * Pitch is front-to-back rotation, where the front of the object moves upwards while the rear of the + * object moves downwards, or vice versa. + *

+ * Roll is side-to-side tilt, where the left side of the object moves upwards while the right side of + * the object moves downwards, or vice versa. + *

+ * All angles are in the range of -180 degrees to 180 degrees. + *

+ * The angles are applied intrinsically, in the order of yaw, then pitch, then roll. "Intrinsically" + * means that the axes move along with the object as you perform the rotations. As an example using + * a robot, if the yaw is 30 degrees, the pitch is 40 degrees, and the roll is 10 degrees, that means + * that you would reach the described orientation by first rotating the object 30 degrees counter-clockwise + * from the starting point, with all wheels continuing to touch the ground (rotation around the Z + * axis, as defined in the Robot Coordinate System). Then, you make your robot point 40 degrees upward + * (rotate it 40 degrees around the X axis, as defined in the Robot Coordinate System). Because the X + * axis moved with the robot, the pitch is not affected by the yaw value. Then from that position, the + * robot is tilted 10 degrees to the right, around the newly positioned Y axis, to produce the actual + * position of the robot. + */ +public class YawPitchRollAngles { + private final AngleUnit angleUnit; + private final double yaw; + private final double pitch; + private final double roll; + private final long acquisitionTime; + + /** + * See the top-level class Javadoc for the format that these angles need to be in. + */ + public YawPitchRollAngles(AngleUnit angleUnit, double yaw, double pitch, double roll, long acquisitionTime) { + this.angleUnit = angleUnit; + this.yaw = yaw; + this.pitch = pitch; + this.roll = roll; + this.acquisitionTime = acquisitionTime; + } + + /** + * @return The side-to-side lateral rotation of the object, + * normalized to the range of [-180,+180) degrees. + */ + public double getYaw() { + return yaw; + } + + /** + * @param angleUnit The unit that will be used for the result. + * @return The side-to-side lateral rotation of the object, + * normalized to the range of [-180,+180) degrees. + */ + public double getYaw(AngleUnit angleUnit) { + return angleUnit.fromUnit(this.angleUnit, yaw); + } + + /** + * @return The front-to-back rotation of the object, normalized to + * the range of [-180,+180) degrees + */ + public double getPitch() { + return pitch; + } + + /** + * @param angleUnit The unit that will be used for the result. + * @return The front-to-back rotation of the object, normalized to + * the range of [-180,+180) degrees + */ + public double getPitch(AngleUnit angleUnit) { + return angleUnit.fromUnit(this.angleUnit, pitch); + } + + /** + * @return The side-to-side tilt of the object, normalized to + * the range of [-180,+180) degrees + */ + public double getRoll() { + return roll; + } + + /** + * @param angleUnit The unit that will be used for the result. + * @return The side-to-side tilt of the object, normalized to + * the range of [-180,+180) degrees + */ + public double getRoll(AngleUnit angleUnit) { + return angleUnit.fromUnit(this.angleUnit, roll); + } + + /** + * @return The time on the System.nanoTime() clock at which the data was acquired. If no + * timestamp is associated with this particular set of data, this value is zero. + */ + public long getAcquisitionTime() { + return acquisitionTime; + } + + @Override + public String toString() { + return String.format(Locale.US, "{yaw=%.3f, pitch=%.3f, roll=%.3f}", + angleUnit.toDegrees(yaw), angleUnit.toDegrees(pitch), angleUnit.toDegrees(roll)); + } +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/internal/camera/calibration/CameraCalibration.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/internal/camera/calibration/CameraCalibration.java index 9e85951b..a2180236 100644 --- a/Common/src/main/java/org/firstinspires/ftc/robotcore/internal/camera/calibration/CameraCalibration.java +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/internal/camera/calibration/CameraCalibration.java @@ -55,6 +55,7 @@ public class CameraCalibration extends CameraIntrinsics implements Cloneable protected Size size; protected boolean remove; protected final boolean isFake; + public Size resolutionScaledFrom; @Override public String toString() { @@ -148,6 +149,7 @@ public CameraCalibration scaledTo(Size newSize) result.focalLengthY *= factor; result.principalPointX *= factor; result.principalPointY *= factor; + result.resolutionScaledFrom = size; return result; } @@ -169,4 +171,4 @@ protected static double getAspectRatio(Size size) return (double)size.getWidth() / (double)size.getHeight(); } -} \ No newline at end of file +} diff --git a/Common/src/main/java/org/firstinspires/ftc/robotcore/robocol/TelemetryMessage.java b/Common/src/main/java/org/firstinspires/ftc/robotcore/robocol/TelemetryMessage.java new file mode 100644 index 00000000..41a62006 --- /dev/null +++ b/Common/src/main/java/org/firstinspires/ftc/robotcore/robocol/TelemetryMessage.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.robotcore.robocol; + +/** + * Placeholder for telemetry message constants + */ +public class TelemetryMessage { + + static final int cbTimestamp = 8; + static final int cbSorted = 1; + static final int cbRobotState = 1; + static final int cbTagLen = 1; + static final int cbCountLen = 1; + static final int cbKeyLen = 2; + static final int cbValueLen = 2; + static final int cbFloat = 4; + + public final static int cbTagMax = (1 << (cbTagLen*8)) - 1; + public final static int cCountMax = (1 << (cbCountLen*8)) - 1; + public final static int cbKeyMax = (1 << (cbKeyLen*8)) - 1; + public final static int cbValueMax = (1 << (cbValueLen*8)) - 1; + +} diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/EOCVSim.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/EOCVSim.kt index ffb1e9e9..85ae0517 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/EOCVSim.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/EOCVSim.kt @@ -388,43 +388,11 @@ class EOCVSim(val params: Parameters = Parameters()) { inputSourceManager.update(pipelineManager.paused) tunerManager.update() - try { - pipelineManager.update( - if (inputSourceManager.lastMatFromSource != null && !inputSourceManager.lastMatFromSource.empty()) { - inputSourceManager.lastMatFromSource - } else null - ) - } catch (ex: MaxActiveContextsException) { //handles when a lot of pipelines are stuck in the background - visualizer.asyncPleaseWaitDialog( - "There are many pipelines stuck in processFrame running in the background", - "To avoid further issues, EOCV-Sim will exit now.", - "Ok", - Dimension(450, 150), - true, - true - ).onCancel { - destroy(DestroyReason.CRASH) //destroy eocv sim when pressing "exit" - } - - //print exception - logger.error( - "Please note that the following exception is likely to be caused by one or more of the user pipelines", - ex - ) - - //block the current thread until the user closes the dialog - try { - //using sleep for avoiding burning cpu cycles - Thread.sleep(Long.MAX_VALUE) - } catch (ignored: InterruptedException) { - //reinterrupt once user closes the dialog - Thread.currentThread().interrupt() - } - - break //bye bye - } catch (ex: InterruptedException) { - break // bye bye - } + pipelineManager.update( + if (inputSourceManager.lastMatFromSource != null && !inputSourceManager.lastMatFromSource.empty()) { + inputSourceManager.lastMatFromSource + } else null + ) //limit FPG fpsLimiter.maxFPS = config.pipelineMaxFps.fps.toDouble() diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/DialogFactory.java b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/DialogFactory.java index e080714d..80f7f09c 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/DialogFactory.java +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/DialogFactory.java @@ -39,6 +39,9 @@ import java.awt.*; import java.io.File; import java.util.ArrayList; +import java.util.concurrent.Executor; +import java.util.concurrent.Executors; +import java.util.concurrent.ThreadPoolExecutor; import java.util.function.IntConsumer; public class DialogFactory { @@ -57,7 +60,7 @@ public static void createYesOrNo(Component parent, String message, String submes panel.setLayout(new GridLayout(2, 1)); } - SwingUtilities.invokeLater(() -> result.accept( + invokeLater(() -> result.accept( JOptionPane.showConfirmDialog(parent, panel, "Confirm", JOptionPane.YES_NO_OPTION, JOptionPane.PLAIN_MESSAGE diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/IconDelegate.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/IconDelegate.kt new file mode 100644 index 00000000..b0be894d --- /dev/null +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/IconDelegate.kt @@ -0,0 +1,15 @@ +package com.github.serivesmejia.eocvsim.gui + +import kotlin.reflect.KProperty + +fun icon(name: String, path: String, allowInvert: Boolean = true) = IconDelegate(name, path, allowInvert) + +class IconDelegate(val name: String, val path: String, allowInvert: Boolean = true) { + init { + Icons.addFutureImage(name, path, allowInvert) + } + + operator fun getValue(any: Any, property: KProperty<*>): Icons.NamedImageIcon { + return Icons.getImage(name) + } +} \ No newline at end of file diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/IconLibrary.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/IconLibrary.kt deleted file mode 100644 index b5d95097..00000000 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/IconLibrary.kt +++ /dev/null @@ -1,17 +0,0 @@ -package com.github.serivesmejia.eocvsim.gui - -import java.awt.image.BufferedImage -import javax.swing.ImageIcon -import kotlin.reflect.KProperty - -fun icon(name: String, path: String, allowInvert: Boolean = true) = EOCVSimIconDelegate(name, path, allowInvert) - -class EOCVSimIconDelegate(val name: String, val path: String, allowInvert: Boolean = true) { - init { - Icons.addFutureImage(name, path, allowInvert) - } - - operator fun getValue(eocvSimIconLibrary: EOCVSimIconLibrary, property: KProperty<*>): Icons.NamedImageIcon { - return Icons.getImage(name) - } -} \ No newline at end of file diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/Visualizer.java b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/Visualizer.java index 4a14718e..3b9f6512 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/Visualizer.java +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/Visualizer.java @@ -266,7 +266,7 @@ public void mouseClicked(MouseEvent e) { public void joinInit() { while (!hasFinishedInitializing) { - Thread.yield(); + Thread.onSpinWait(); } } @@ -341,7 +341,7 @@ public void asyncCompilePipelines() { menuBar.workspCompile.setEnabled(false); pipelineSelectorPanel.getButtonsPanel().getPipelineCompileBtt().setEnabled(false); - eocvSim.pipelineManager.compiledPipelineManager.asyncCompile(true, (result) -> { + eocvSim.pipelineManager.compiledPipelineManager.asyncCompile((result) -> { menuBar.workspCompile.setEnabled(true); pipelineSelectorPanel.getButtonsPanel().getPipelineCompileBtt().setEnabled(true); diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/tuner/TunableFieldPanel.java b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/tuner/TunableFieldPanel.java index 523e2e87..38618973 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/tuner/TunableFieldPanel.java +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/tuner/TunableFieldPanel.java @@ -33,7 +33,7 @@ import javax.swing.border.SoftBevelBorder; import java.awt.*; -@SuppressWarnings("Unchecked") +@SuppressWarnings("unchecked") public class TunableFieldPanel extends JPanel { public final TunableField tunableField; diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/visualizer/PipelineOpModeSwitchablePanel.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/visualizer/PipelineOpModeSwitchablePanel.kt index f9f4e52d..80d1d651 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/visualizer/PipelineOpModeSwitchablePanel.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/gui/component/visualizer/PipelineOpModeSwitchablePanel.kt @@ -115,6 +115,10 @@ class PipelineOpModeSwitchablePanel(val eocvSim: EOCVSim) : JTabbedPane() { pipelineSelectorPanel.isActive = false opModeSelectorPanel.isActive = true + } else { + opModeSelectorPanel.reset() + pipelineSelectorPanel.isActive = false + opModeSelectorPanel.isActive = false } } } diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/DefaultPipeline.java b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/DefaultPipeline.java index 47f0b24d..c4c11323 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/DefaultPipeline.java +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/DefaultPipeline.java @@ -75,7 +75,7 @@ public Mat processFrame(Mat input) { @Override public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { - canvas.drawRect(new Rect(0, 0, 385, 45), boxPaint); + canvas.drawRect(new Rect(0, 0, 345, 45), boxPaint); canvas.drawText("Default pipeline selected", 5, 33, textPaint); } diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/PipelineManager.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/PipelineManager.kt index e3b3757f..c9a910dd 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/PipelineManager.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/PipelineManager.kt @@ -35,26 +35,22 @@ import com.github.serivesmejia.eocvsim.pipeline.util.PipelineSnapshot import com.github.serivesmejia.eocvsim.util.ReflectUtil import com.github.serivesmejia.eocvsim.util.StrUtil import com.github.serivesmejia.eocvsim.util.event.EventHandler -import com.github.serivesmejia.eocvsim.util.exception.MaxActiveContextsException import com.github.serivesmejia.eocvsim.util.fps.FpsCounter import com.github.serivesmejia.eocvsim.util.loggerForThis import io.github.deltacv.common.image.MatPoster import io.github.deltacv.common.pipeline.util.PipelineStatisticsCalculator import io.github.deltacv.eocvsim.virtualreflect.VirtualField -import io.github.deltacv.eocvsim.virtualreflect.VirtualReflectContext import io.github.deltacv.eocvsim.virtualreflect.VirtualReflection import io.github.deltacv.eocvsim.virtualreflect.jvm.JvmVirtualReflection import kotlinx.coroutines.* import org.firstinspires.ftc.robotcore.external.Telemetry -import org.firstinspires.ftc.robotcore.internal.opmode.TelemetryImpl +import org.firstinspires.ftc.robotcore.internal.opmode.EOCVSimTelemetryImpl import org.firstinspires.ftc.vision.VisionProcessor import org.opencv.core.Mat import org.openftc.easyopencv.OpenCvPipeline import org.openftc.easyopencv.OpenCvViewport import org.openftc.easyopencv.processFrameInternal import java.lang.RuntimeException -import java.lang.reflect.Constructor -import java.lang.reflect.Field import java.util.* import kotlin.coroutines.EmptyCoroutineContext import kotlin.math.roundToLong @@ -202,7 +198,7 @@ class PipelineManager( openedPipelineOutputCount++ } - if(telemetry is TelemetryImpl) { + if(telemetry is EOCVSimTelemetryImpl) { telemetry.errItem.caption = "[/!\\]" telemetry.errItem.setValue("Uncaught exception thrown, check Workspace -> Output.") telemetry.forceTelemetryTransmission() @@ -212,7 +208,7 @@ class PipelineManager( pipelineExceptionTracker.onPipelineExceptionClear { val telemetry = currentTelemetry - if(telemetry is TelemetryImpl) { + if(telemetry is EOCVSimTelemetryImpl) { telemetry.errItem.caption = "" telemetry.errItem.setValue("") telemetry.forceTelemetryTransmission() @@ -275,11 +271,7 @@ class PipelineManager( } } - if(activePipelineContexts.size > MAX_ALLOWED_ACTIVE_PIPELINE_CONTEXTS) { - throw MaxActiveContextsException("Current amount of active pipeline coroutine contexts (${activePipelineContexts.size}) is more than the maximum allowed. This generally means that there are multiple pipelines stuck in processFrame() running in the background, check for any lengthy operations in your pipelines.") - } - - if(telemetry is TelemetryImpl) { + if(telemetry is EOCVSimTelemetryImpl) { if (compiledPipelineManager.isBuildRunning) { telemetry.infoItem.caption = "[>]" telemetry.infoItem.setValue("Building java files in workspace...") @@ -585,7 +577,7 @@ class PipelineManager( val instantiator = getInstantiatorFor(pipelineClass) try { - nextTelemetry = TelemetryImpl().apply { + nextTelemetry = EOCVSimTelemetryImpl().apply { // send telemetry updates to the ui addTransmissionReceiver(eocvSim.visualizer.telemetryPanel) } diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/compiler/CompiledPipelineManager.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/compiler/CompiledPipelineManager.kt index 1f43b1fc..08798cf0 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/compiler/CompiledPipelineManager.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/compiler/CompiledPipelineManager.kt @@ -88,7 +88,7 @@ class CompiledPipelineManager(private val pipelineManager: PipelineManager) { fun init() { logger.info("Initializing...") - asyncCompile(false) + asyncCompile() workspaceManager.onWorkspaceChange { asyncCompile() @@ -189,7 +189,7 @@ class CompiledPipelineManager(private val pipelineManager: PipelineManager) { return result } - fun compile(fixSelectedPipeline: Boolean = true) = try { + fun compile() = try { runBlocking { uncheckedCompile() } } catch(e: Throwable) { isBuildRunning = false @@ -200,7 +200,7 @@ class CompiledPipelineManager(private val pipelineManager: PipelineManager) { |Unexpected exception thrown while the build was running | |$stacktrace - | + | |If this seems like a bug, please open an issue in the EOCV-Sim github repo """.trimMargin() @@ -217,10 +217,9 @@ class CompiledPipelineManager(private val pipelineManager: PipelineManager) { @JvmOverloads @OptIn(DelicateCoroutinesApi::class) fun asyncCompile( - fixSelectedPipeline: Boolean = true, endCallback: (PipelineCompileResult) -> Unit = {} ) = GlobalScope.launch(Dispatchers.IO) { - endCallback(compile(fixSelectedPipeline)) + endCallback(compile()) } private fun deleteJarFile() { diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/util/PipelineExceptionTracker.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/util/PipelineExceptionTracker.kt index e992a0d1..8b113626 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/util/PipelineExceptionTracker.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/pipeline/util/PipelineExceptionTracker.kt @@ -32,7 +32,6 @@ class PipelineExceptionTracker(private val pipelineManager: PipelineManager) { companion object { const val millisExceptionExpire = 35000L - const val cutStacktraceLines = 9 } val logger by loggerForThis() diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/ClasspathScan.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/ClasspathScan.kt index 9870d02a..d76ec1d1 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/ClasspathScan.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/ClasspathScan.kt @@ -50,6 +50,7 @@ class ClasspathScan { "io.github.classgraph", "io.github.deltacv", "com.github.serivesmejia.eocvsim.pipeline", + "org.firstinspires.ftc.vision", "org.lwjgl", "org.apache", "org.codehaus", diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/JavaProcess.java b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/JavaProcess.java index 0f6f5ff4..451ad89c 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/JavaProcess.java +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/util/JavaProcess.java @@ -25,25 +25,34 @@ import java.io.File; import java.io.IOException; +import java.io.InputStream; import java.util.LinkedList; import java.util.List; /** - * A utility class for executing a Java process with a class within this project. + * A utility class for executing a Java process to run a main class within this project. */ public final class JavaProcess { + public interface ProcessIOReceiver { + void receive(InputStream in, InputStream err); + } + private JavaProcess() {} /** * Executes a Java process with the given class and arguments. * @param klass the class to execute + * @param ioReceiver the receiver for the process' input and error streams (will use inheritIO if null) + * @param classpath the classpath to use + * (use System.getProperty("java.class.path") for the default classpath) + * @param jvmArgs the JVM arguments to pass to the process * @param args the arguments to pass to the class * @return the exit value of the process * @throws InterruptedException if the process is interrupted * @throws IOException if an I/O error occurs */ - public static int execClasspath(Class klass, String classpath, String... args) throws InterruptedException, IOException { + public static int execClasspath(Class klass, ProcessIOReceiver ioReceiver, String classpath, List jvmArgs, List args) throws InterruptedException, IOException { String javaHome = System.getProperty("java.home"); String javaBin = javaHome + File.separator + "bin" + @@ -52,23 +61,50 @@ public static int execClasspath(Class klass, String classpath, String... args) t List command = new LinkedList<>(); command.add(javaBin); + if (jvmArgs != null) { + command.addAll(jvmArgs); + } command.add("-cp"); command.add(classpath); command.add(className); if (args != null) { - command.addAll(List.of(args)); + command.addAll(args); } ProcessBuilder builder = new ProcessBuilder(command); - Process process = builder.inheritIO().start(); - process.waitFor(); - return process.exitValue(); + if (ioReceiver != null) { + Process process = builder.start(); + ioReceiver.receive(process.getInputStream(), process.getErrorStream()); + killOnExit(process); + + process.waitFor(); + return process.exitValue(); + } else { + builder.inheritIO(); + Process process = builder.start(); + killOnExit(process); + + process.waitFor(); + return process.exitValue(); + } } + private static void killOnExit(Process process) { + Runtime.getRuntime().addShutdownHook(new Thread(process::destroy)); + } - public static int exec(Class klass, String... args) throws InterruptedException, IOException { - return execClasspath(klass, System.getProperty("java.class.path"), args); + /** + * Executes a Java process with the given class and arguments. + * @param klass the class to execute + * @param jvmArgs the JVM arguments to pass to the process + * @param args the arguments to pass to the class + * @return the exit value of the process + * @throws InterruptedException if the process is interrupted + * @throws IOException if an I/O error occurs + */ + public static int exec(Class klass, List jvmArgs, List args) throws InterruptedException, IOException { + return execClasspath(klass, null, System.getProperty("java.class.path"), jvmArgs, args); } } \ No newline at end of file diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/DefaultWorkspaceTemplate.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/DefaultWorkspaceTemplate.kt index 48037d28..2c45e07a 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/DefaultWorkspaceTemplate.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/DefaultWorkspaceTemplate.kt @@ -40,7 +40,7 @@ object DefaultWorkspaceTemplate : WorkspaceTemplate() { if(!folder.isDirectory) return false val templateZipFile = SysUtil.copyFileIsTemp( - templateZipResource, "default_workspace.zip", false + templateZipResource, "default_workspace.zip", true ).file return try { diff --git a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/GradleWorkspaceTemplate.kt b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/GradleWorkspaceTemplate.kt index bcf86329..5fc99be7 100644 --- a/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/GradleWorkspaceTemplate.kt +++ b/EOCV-Sim/src/main/java/com/github/serivesmejia/eocvsim/workspace/util/template/GradleWorkspaceTemplate.kt @@ -43,7 +43,7 @@ object GradleWorkspaceTemplate : WorkspaceTemplate() { if(!folder.isDirectory) return false val templateZipFile = SysUtil.copyFileIsTemp( - templateZipResource, "gradle_workspace.zip", false + templateZipResource, "gradle_workspace.zip", true ).file return try { diff --git a/EOCV-Sim/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModePipelineHandler.kt b/EOCV-Sim/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModePipelineHandler.kt index 06ee88aa..dda9ea55 100644 --- a/EOCV-Sim/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModePipelineHandler.kt +++ b/EOCV-Sim/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModePipelineHandler.kt @@ -45,8 +45,7 @@ class OpModePipelineHandler(val inputSourceManager: InputSourceManager, private ThreadSourceHander.register(VisionInputSourceHander(pipeline?.notifier ?: return, viewport)) pipeline?.telemetry = telemetry - pipeline?.hardwareMap = HardwareMap(); - } + pipeline?.hardwareMap = HardwareMap() } override fun init() { } diff --git a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/gui/dialog/SuperAccessRequest.kt b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/gui/dialog/SuperAccessRequest.kt index 72e7b883..6b8d8074 100644 --- a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/gui/dialog/SuperAccessRequest.kt +++ b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/gui/dialog/SuperAccessRequest.kt @@ -103,7 +103,7 @@ class SuperAccessRequest(sourceName: String, reason: String) { // Setup the frame frame.contentPane = panel frame.isAlwaysOnTop = true - frame.defaultCloseOperation = JDialog.DISPOSE_ON_CLOSE + frame.defaultCloseOperation = JDialog.HIDE_ON_CLOSE frame.isResizable = false frame.addWindowListener(object: WindowListener { diff --git a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginClassLoader.kt b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginClassLoader.kt index daf81e8a..af498d85 100644 --- a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginClassLoader.kt +++ b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginClassLoader.kt @@ -43,7 +43,7 @@ import java.util.zip.ZipFile * @param pluginJar the jar file of the plugin * @param pluginContext the plugin context */ -class PluginClassLoader(private val pluginJar: File, val pluginContext: PluginContext) : ClassLoader() { +class PluginClassLoader(private val pluginJar: File, val pluginContextProvider: () -> PluginContext) : ClassLoader() { private val zipFile = try { ZipFile(pluginJar) @@ -65,7 +65,7 @@ class PluginClassLoader(private val pluginJar: File, val pluginContext: PluginCo SysUtil.copyStream(inStream, outStream) val bytes = outStream.toByteArray() - if(!pluginContext.hasSuperAccess) + if(!pluginContextProvider().hasSuperAccess) MethodCallByteCodeChecker(bytes, dynamicLoadingMethodBlacklist) val clazz = defineClass(name, bytes, 0, bytes.size) @@ -85,7 +85,7 @@ class PluginClassLoader(private val pluginJar: File, val pluginContext: PluginCo * @throws IllegalAccessError if the class is blacklisted */ fun loadClassStrict(name: String): Class<*> { - if(!pluginContext.hasSuperAccess) { + if(!pluginContextProvider().hasSuperAccess) { for (blacklistedPackage in dynamicLoadingPackageBlacklist) { if (name.contains(blacklistedPackage)) { throw IllegalAccessError("Plugins are blacklisted to use $name") @@ -109,7 +109,7 @@ class PluginClassLoader(private val pluginJar: File, val pluginContext: PluginCo } } - if(!inWhitelist && !pluginContext.hasSuperAccess) { + if(!inWhitelist && !pluginContextProvider().hasSuperAccess) { throw IllegalAccessError("Plugins are not whitelisted to use $name") } diff --git a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginContext.kt b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginContext.kt index f071c849..6fe21731 100644 --- a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginContext.kt +++ b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginContext.kt @@ -31,7 +31,7 @@ class PluginContext( val eocvSim: EOCVSim, val fileSystem: SandboxFileSystem, val loader: PluginLoader ) { companion object { - @JvmStatic fun current(plugin: EOCVSimPlugin) = (plugin.javaClass.classLoader as PluginClassLoader).pluginContext + @JvmStatic fun current(plugin: EOCVSimPlugin) = (plugin.javaClass.classLoader as PluginClassLoader).pluginContextProvider() } val plugin get() = loader.plugin diff --git a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginLoader.kt b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginLoader.kt index 004a2420..c47ebafe 100644 --- a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginLoader.kt +++ b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginLoader.kt @@ -78,8 +78,8 @@ class PluginLoader(val pluginFile: File, val eocvSim: EOCVSim) { lateinit var fileSystem: SandboxFileSystem private set - val fileSystemZip = PluginManager.FILESYSTEMS_FOLDER + File.separator + "${hash()}-fs" - val fileSystemZipPath = fileSystemZip.toPath() + val fileSystemZip by lazy { PluginManager.FILESYSTEMS_FOLDER + File.separator + "${hash()}-fs" } + val fileSystemZipPath by lazy { fileSystemZip.toPath() } /** * Whether the plugin has super access (full system access) @@ -87,8 +87,9 @@ class PluginLoader(val pluginFile: File, val eocvSim: EOCVSim) { val hasSuperAccess get() = eocvSim.config.superAccessPluginHashes.contains(pluginHash) init { - setupFs() - pluginClassLoader = PluginClassLoader(pluginFile, PluginContext(eocvSim, fileSystem, this)) + pluginClassLoader = PluginClassLoader(pluginFile) { + PluginContext(eocvSim, fileSystem, this) + } } /** @@ -111,6 +112,8 @@ class PluginLoader(val pluginFile: File, val eocvSim: EOCVSim) { logger.info("Loading plugin $pluginName v$pluginVersion by $pluginAuthor") + setupFs() + if(pluginToml.contains("api-version")) { val parsedVersion = ParsedVersion(pluginToml.getString("api-version")) @@ -118,7 +121,7 @@ class PluginLoader(val pluginFile: File, val eocvSim: EOCVSim) { throw UnsupportedPluginException("Plugin request api version of v${parsedVersion}, EOCV-Sim is currently running at v${EOCVSim.PARSED_VERSION}") } - if(pluginToml.contains("super-access") && pluginToml.getBoolean("super-access", false)) { + if(pluginToml.getBoolean("super-access", false)) { requestSuperAccess(pluginToml.getString("super-access-reason", "")) } @@ -190,12 +193,12 @@ class PluginLoader(val pluginFile: File, val eocvSim: EOCVSim) { } /** - * Get the hash of the plugin file based off the absolute path + * Get the hash of the plugin file based off the plugin name and author * @return the hash */ fun hash(): String { val messageDigest = MessageDigest.getInstance("SHA-256") - messageDigest.update(pluginFile.absolutePath.encodeToByteArray()) + messageDigest.update("${pluginName} by ${pluginAuthor}".toByteArray()) return SysUtil.byteArray2Hex(messageDigest.digest()) } diff --git a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginManager.kt b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginManager.kt index c1402cbf..ff137677 100644 --- a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginManager.kt +++ b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/plugin/loader/PluginManager.kt @@ -30,6 +30,7 @@ import com.github.serivesmejia.eocvsim.util.io.EOCVSimFolder import com.github.serivesmejia.eocvsim.util.loggerForThis import io.github.deltacv.eocvsim.gui.dialog.SuperAccessRequestMain import java.io.File +import java.util.* /** * Manages the loading, enabling and disabling of plugins @@ -145,6 +146,8 @@ class PluginManager(val eocvSim: EOCVSim) { fun requestSuperAccessFor(loader: PluginLoader, reason: String): Boolean { if(loader.hasSuperAccess) return true + logger.info("Requesting super access for ${loader.pluginName} v${loader.pluginVersion}") + var warning = "$GENERIC_SUPERACCESS_WARN" if(reason.trim().isNotBlank()) { warning += "

$reason" @@ -156,7 +159,7 @@ class PluginManager(val eocvSim: EOCVSim) { val name = "${loader.pluginName} by ${loader.pluginAuthor}".replace(" ", "-") - if(JavaProcess.exec(SuperAccessRequestMain::class.java, name, warning) == 171) { + if(JavaProcess.exec(SuperAccessRequestMain::class.java, null, Arrays.asList(name, warning)) == 171) { eocvSim.config.superAccessPluginHashes.add(loader.pluginHash) eocvSim.configManager.saveToFile() return true diff --git a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/virtualreflect/jvm/Label.java b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/virtualreflect/jvm/Label.java index 0b1af0f2..18069124 100644 --- a/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/virtualreflect/jvm/Label.java +++ b/EOCV-Sim/src/main/java/io/github/deltacv/eocvsim/virtualreflect/jvm/Label.java @@ -1,3 +1,26 @@ +/* + * Copyright (c) 2024 Sebastian Erives + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + package io.github.deltacv.eocvsim.virtualreflect.jvm; import java.lang.annotation.ElementType; diff --git a/EOCV-Sim/src/main/java/org/firstinspires/ftc/robotcore/internal/opmode/TelemetryImpl.java b/EOCV-Sim/src/main/java/org/firstinspires/ftc/robotcore/internal/opmode/EOCVSimTelemetryImpl.java similarity index 93% rename from EOCV-Sim/src/main/java/org/firstinspires/ftc/robotcore/internal/opmode/TelemetryImpl.java rename to EOCV-Sim/src/main/java/org/firstinspires/ftc/robotcore/internal/opmode/EOCVSimTelemetryImpl.java index 8e21e920..1af5a2f2 100644 --- a/EOCV-Sim/src/main/java/org/firstinspires/ftc/robotcore/internal/opmode/TelemetryImpl.java +++ b/EOCV-Sim/src/main/java/org/firstinspires/ftc/robotcore/internal/opmode/EOCVSimTelemetryImpl.java @@ -1,18 +1,23 @@ /* Copyright (c) 2016 Robert Atkinson + All rights reserved. -Adapted for EOCV-Sim (c) 2021 Sebastian Erives + Redistribution and use in source and binary forms, with or without modification, are permitted (subject to the limitations in the disclaimer below) provided that the following conditions are met: + Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + Neither the name of Robert Atkinson nor the names of his contributors may be used to endorse or promote products derived from this software without specific prior written permission. + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, @@ -27,24 +32,29 @@ are permitted (subject to the limitations in the disclaimer below) provided that */ package org.firstinspires.ftc.robotcore.internal.opmode; -import com.github.serivesmejia.eocvsim.gui.component.visualizer.TelemetryPanel; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Func; import org.firstinspires.ftc.robotcore.external.Predicate; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.robocol.TelemetryMessage; -import javax.swing.*; +import java.text.DecimalFormat; import java.util.ArrayList; import java.util.Iterator; import java.util.LinkedList; import java.util.List; /** - * {@link TelemetryImpl} is the system-provided implementation of the {@link Telemetry} interface. + * {@link EOCVSimTelemetryImpl} is the system-provided implementation of the {@link Telemetry} interface. */ -public class TelemetryImpl implements Telemetry, TelemetryInternal +public class EOCVSimTelemetryImpl implements Telemetry, TelemetryInternal { + //---------------------------------------------------------------------------------------------- // Types //---------------------------------------------------------------------------------------------- @@ -78,9 +88,12 @@ protected class Value this.valueProducer = valueProducer; } - Value(Object value) - { - this.value = value; + Value(Object value) { + if ((value instanceof Double) || (value instanceof Float)) { + this.value = decimalFormat.format(value); + } else { + this.value = value; + } } Value(Func valueProducer) @@ -97,7 +110,7 @@ boolean isProducer() return this.valueProducer != null; } - String getComposed(boolean recompose) + @NonNull String getComposed(boolean recompose) { if (recompose || composed==null) { @@ -142,13 +155,7 @@ void boundedAddToList(int index, Lineable data) // Using the max # of data items that can be actually transmitted, ever, seems like // a practical choice as an upper bound. - - // 255 was inlined from the original calculations in robocol's TelemetryMessage.cCountMax: - // - // static final int cbCountLen = 1; - // ... - // public final static int cCountMax = (1 << (cbCountLen*8)) - 1; - if (list.size() < 255) + if (list.size() < TelemetryMessage.cCountMax) { list.add(index, data); } @@ -270,6 +277,7 @@ protected class ItemImpl implements Item, Lineable String caption = null; Value value = null; Boolean retained = null; + boolean showIfEmpty = true; //------------------------------------------------------------------------------------------ @@ -294,7 +302,7 @@ protected class ItemImpl implements Item, Lineable { String composed = this.value.getComposed(recompose); - if(!showIfEmpty && this.caption.trim().equals("") && composed.trim().equals("")) { + if(!showIfEmpty && this.caption.trim().isEmpty() && composed.trim().isEmpty()) { return ""; } @@ -323,7 +331,7 @@ protected class ItemImpl implements Item, Lineable } } - @Override public Item setRetained(Boolean retained) + @Override public Item setRetained(@Nullable Boolean retained) { synchronized (theLock) { @@ -418,14 +426,14 @@ protected class LineImpl implements Line, Lineable // Operations //------------------------------------------------------------------------------------------ - public String getComposed(boolean recompose, boolean appendSeparator) + @Override public String getComposed(boolean recompose) { StringBuilder result = new StringBuilder(); result.append(this.lineCaption); boolean firstTime = true; for (Lineable lineable : lineables) { - if (!firstTime && appendSeparator) + if (!firstTime) { result.append(getItemSeparator()); } @@ -435,11 +443,6 @@ public String getComposed(boolean recompose, boolean appendSeparator) return result.toString(); } - @Override - public String getComposed(boolean recompose) { - return getComposed(recompose, true); - } - @Override public Item addData(String caption, String format, Object... args) { return lineables.addItemAfter(null, caption, new Value(format, args)); @@ -459,10 +462,6 @@ public String getComposed(boolean recompose) { { return lineables.addItemAfter(null, caption, new Value(format, valueProducer)); } - - public boolean isEmpty() { - return getComposed(false, false).trim().equals(""); - } } protected class LogImpl implements Log @@ -510,7 +509,7 @@ boolean isDirty() // Use the outer class to mindlessly avoid any potential deadlocks Object getLock() { - return TelemetryImpl.this; + return EOCVSimTelemetryImpl.this; } int size() @@ -614,12 +613,16 @@ void reset() protected ElapsedTime transmissionTimer; protected boolean isDirty; protected boolean clearOnAdd; + protected OpMode opMode; protected boolean isAutoClear; protected int msTransmissionInterval; protected String captionValueSeparator; protected String itemSeparator; - protected StringBuilder currentSb; + protected DecimalFormat decimalFormat = new DecimalFormat("0.####"); + /* + * EOCV-Sim + */ protected ArrayList transmissionReceivers = new ArrayList<>(); public Item errItem; @@ -629,7 +632,7 @@ void reset() // Construction //---------------------------------------------------------------------------------------------- - public TelemetryImpl() + public EOCVSimTelemetryImpl() { this.log = new LogImpl(); resetTelemetryForOpMode(); @@ -649,8 +652,7 @@ public void resetTelemetryForOpMode() this.msTransmissionInterval = 250; this.captionValueSeparator = " : "; this.itemSeparator = " | "; - this.currentSb = new StringBuilder(); - this.transmissionReceivers = new ArrayList<>(); + errItem = addData("", "").setRetained(true); ((ItemImpl)errItem).showIfEmpty = false; @@ -678,6 +680,11 @@ boolean isDirty() return this.isDirty; } + public void setNumDecimalPlaces(int minDecimalPlaces, int maxDecimalPlaces){ + decimalFormat.setMinimumFractionDigits(minDecimalPlaces); + decimalFormat.setMaximumFractionDigits(maxDecimalPlaces); + } + //---------------------------------------------------------------------------------------------- // Updating //---------------------------------------------------------------------------------------------- @@ -753,11 +760,14 @@ else if (updateReason==UpdateReason.USER) return result; } + + } + protected void saveToTransmitter(boolean recompose) { - currentSb = new StringBuilder(); + StringBuilder currentSb = new StringBuilder(); // When we recompose, we save the composed lines. Thus, they will stick around // even after we might get clear()'d. In that way, they'll still be there to @@ -870,6 +880,11 @@ public boolean removeTransmissionReceiver(TelemetryTransmissionReceiver transmis } } + @Override public void setDisplayFormat(DisplayFormat displayFormat) + { + // no-op in eocv-sim + } + //---------------------------------------------------------------------------------------------- // Adding and removing data //---------------------------------------------------------------------------------------------- @@ -984,9 +999,17 @@ protected void onAddData() } } - @Override - public String toString() { - return currentSb.toString(); + //---------------------------------------------------------------------------------------------- + // Text to Speech + //---------------------------------------------------------------------------------------------- + + @Override public void speak(String text) + { + speak(text, null, null); } -} \ No newline at end of file + @Override public void speak(String text, String languageCode, String countryCode) + { + // no-op + } +} diff --git a/EOCV-Sim/src/main/resources/fonts/LICENSE.txt b/EOCV-Sim/src/main/resources/fonts/LICENSE.txt new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/EOCV-Sim/src/main/resources/fonts/LICENSE.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/EOCV-Sim/src/main/resources/fonts/Roboto-Bold.ttf b/EOCV-Sim/src/main/resources/fonts/Roboto-Bold.ttf new file mode 100644 index 00000000..e64db796 Binary files /dev/null and b/EOCV-Sim/src/main/resources/fonts/Roboto-Bold.ttf differ diff --git a/EOCV-Sim/src/main/resources/fonts/Roboto-BoldItalic.ttf b/EOCV-Sim/src/main/resources/fonts/Roboto-BoldItalic.ttf new file mode 100644 index 00000000..5e39ae97 Binary files /dev/null and b/EOCV-Sim/src/main/resources/fonts/Roboto-BoldItalic.ttf differ diff --git a/EOCV-Sim/src/main/resources/fonts/Roboto-Italic.ttf b/EOCV-Sim/src/main/resources/fonts/Roboto-Italic.ttf new file mode 100644 index 00000000..65498ee3 Binary files /dev/null and b/EOCV-Sim/src/main/resources/fonts/Roboto-Italic.ttf differ diff --git a/EOCV-Sim/src/main/resources/fonts/Roboto-Regular.ttf b/EOCV-Sim/src/main/resources/fonts/Roboto-Regular.ttf new file mode 100644 index 00000000..2d116d92 Binary files /dev/null and b/EOCV-Sim/src/main/resources/fonts/Roboto-Regular.ttf differ diff --git a/EOCV-Sim/src/main/resources/templates/default_workspace.zip b/EOCV-Sim/src/main/resources/templates/default_workspace.zip index 2afb2646..9d41330a 100644 Binary files a/EOCV-Sim/src/main/resources/templates/default_workspace.zip and b/EOCV-Sim/src/main/resources/templates/default_workspace.zip differ diff --git a/EOCV-Sim/src/main/resources/templates/gradle_workspace.zip b/EOCV-Sim/src/main/resources/templates/gradle_workspace.zip index 59480ce0..29a5cc7d 100644 Binary files a/EOCV-Sim/src/main/resources/templates/gradle_workspace.zip and b/EOCV-Sim/src/main/resources/templates/gradle_workspace.zip differ diff --git a/README.md b/README.md index 7d277903..7c2cb944 100644 --- a/README.md +++ b/README.md @@ -47,7 +47,9 @@ Since OpenCV in Java uses a native library, which is platform specific, the simu Follow the steps in [this page](https://deltacv.gitbook.io/eocv-sim/basics/downloading-eocv-sim) to download the sim. The rest of the documentation can also be found [there](https://deltacv.gitbook.io/eocv-sim/). -## Adding EOCV-Sim as a dependency +## Adding EOCV-Sim as a dependency for plugin development + +### NOT FOR FTC SDK USAGE, please follow the documentation above if you're a normal user not aiming to develop for EOCV-Sim ### Gradle ```groovy @@ -56,7 +58,7 @@ Follow the steps in [this page](https://deltacv.gitbook.io/eocv-sim/basics/downl } dependencies { - implementation 'com.github.deltacv:EOCV-Sim:3.3.2' //add the EOCV-Sim dependency + implementation 'com.github.deltacv:EOCV-Sim:3.3.2' // add the EOCV-Sim dependency, make sure to replace for the latest version } ``` @@ -84,12 +86,29 @@ Follow the steps in [this page](https://deltacv.gitbook.io/eocv-sim/basics/downl # Contact For bug reporting or feature requesting, use the [issues tab](https://github.com/deltacv/EOCV-Sim/issues) in this repository. +Join the [deltacv discord server](https://discord.gg/A3RMYzf6DA) ! + # Change logs ### Formerly, EOCV-Sim was hosted on a [personal account repo](https://github.com/serivesmejia/EOCV-Sim/). Released prior to 3.0.0 can be found there for historic purposes. +## [v3.7.1 - Better FTC VisionPortal support & Plugin System Fixes](https://github.com/deltacv/EOCV-Sim/releases/tag/v3.7.1) +- This is the 24th release for EOCV-Sim + - Changelog + - Update skiko to 0.8.15 + - Fixes Typeface.DEFAULT_BOLD and Typeface.DEFAULT_ITALIC to actually work + - Adds a stub no-op implementation for the FTC SDK Gamepad class into OpMode + - Adds android.opengl.Matrix implementation and matrices from the FTC SDK + - Adds navigation classes from the FTC SDK (AngleUnit, AxesOrder, AxesReference, etc) + - Adds the ConceptAprilTagLocalization, ConceptVisionColorLocator and ConceptVisionColorSensor samples + - Reimplements Telemetry to EOCVSimTelemetryImpl with stubs for Telemetry#talk + - Internal changes: + - Plugin virtual filesystems now use the name and author in the TOML to generate the fs name + - Allows to specify JVM args in JavaExec + - Rename some internal classes + - Better handling of Pipeline/OpMode tab switching -### [v3.7.0 - FTC SDK 10.1 & Refined Plugin System](https://github.com/deltacv/EOCV-Sim/releases/tag/v3.5.4) +### [v3.7.0 - FTC SDK 10.1 & Refined Plugin System](https://github.com/deltacv/EOCV-Sim/releases/tag/v3.7.0) - This is the 23nd release for EOCV-Sim - Changelog - Addresses the changes made in the FTC SDK 10.1 for the 2024-2025 season: @@ -101,7 +120,7 @@ For bug reporting or feature requesting, use the [issues tab](https://github.com - Bugfixes: - Fixes exception loop when an exception is thrown from pipeline init -### [v3.6.0 - Plugin System & Into the Deep AprilTags](https://github.com/deltacv/EOCV-Sim/releases/tag/v3.5.4) +### [v3.6.0 - Plugin System & Into the Deep AprilTags](https://github.com/deltacv/EOCV-Sim/releases/tag/v3.6.0) - This is the 22nd release for EOCV-Sim - Changelog - Addresses the changes made in the FTC SDK 10.0 for the 2024-2025 season: diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index a1e9743b..be3aaf0e 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -8,13 +8,12 @@ apply from: '../build.common.gradle' dependencies { implementation project(':EOCV-Sim') - implementation "com.github.deltacv.AprilTagDesktop:AprilTagDesktop:$apriltag_plugin_version" - implementation "org.jetbrains.kotlin:kotlin-stdlib" } -task(runSim, dependsOn: 'classes', type: JavaExec) { - main = 'com.github.serivesmejia.eocvsim.Main' +tasks.register('runSim', JavaExec) { + dependsOn 'classes' + mainClass = 'com.github.serivesmejia.eocvsim.Main' classpath = sourceSets.main.runtimeClasspath } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java new file mode 100644 index 00000000..e0f65f3a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -0,0 +1,240 @@ +/* Copyright (c) 2024 Dryw Wade. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag based localization. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will be used to compute the robot's location and orientation. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin. + * This information is provided in the "robotPose" member of the returned "detection". + * + * To learn about the Field Coordinate System that is defined for FTC (and used by this OpMode), see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/game_specific_resources/field_coordinate_system/field-coordinate-system.html + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Localization", group = "Concept") +@Disabled +public class ConceptAprilTagLocalization extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * Variables to store the position and orientation of the camera on the robot. Setting these + * values requires a definition of the axes of the camera and robot: + * + * Camera axes: + * Origin location: Center of the lens + * Axes orientation: +x right, +y down, +z forward (from camera's perspective) + * + * Robot axes (this is typical, but you can define this however you want): + * Origin location: Center of the robot at field height + * Axes orientation: +x right, +y forward, +z upward + * + * Position: + * If all values are zero (no translation), that implies the camera is at the center of the + * robot. Suppose your camera is positioned 5 inches to the left, 7 inches forward, and 12 + * inches above the ground - you would need to set the position to (-5, 7, 12). + * + * Orientation: + * If all values are zero (no rotation), that implies the camera is pointing straight up. In + * most cases, you'll need to set the pitch to -90 degrees (rotation about the x-axis), meaning + * the camera is horizontal. Use a yaw of 0 if the camera is pointing forwards, +90 degrees if + * it's pointing straight left, -90 degrees for straight right, etc. You can also set the roll + * to +/-90 degrees if it's vertical, or 180 degrees if it's upside-down. + */ + private Position cameraPosition = new Position(DistanceUnit.INCH, + 0, 0, 0, 0); + private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, + 0, -90, 0, 0); + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Share the CPU. + sleep(20); + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + .setCameraPose(cameraPosition, cameraOrientation) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + + } // end method telemetryAprilTag() + +} // end class \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java b/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java new file mode 100644 index 00000000..312120c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java @@ -0,0 +1,191 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.SortOrder; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.opencv.core.RotatedRect; + +import java.util.List; + +/* + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions + * + * Unlike a "color sensor" which determines the color of an object in the field of view, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that match the requested color range. + * These blobs can be further filtered and sorted to find the one most likely to be the item the user is looking for. + * + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. + * The ColorBlobLocatorProcessor process is created first, and then the VisionPortal is built to use this process. + * The ColorBlobLocatorProcessor analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. The outer boundaries of these blobs are called its "contour". + * For each blob, the process then creates the smallest possible rectangle "boxFit" that will fully encase the contour. + * The user can then call getBlobs() to retrieve the list of Blobs, where each Blob contains the contour and the boxFit data. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest contours are listed first. + * + * To aid the user, a colored boxFit rectangle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Locator", group = "Concept") +public class ConceptVisionColorLocator extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. + * - Specify the color range you are looking for. You can use a predefined color, or create you own color range + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available predefined colors are: RED, BLUE YELLOW GREEN + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) + * + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height square centered on screen + * + * - Define which contours are included. + * You can get ALL the contours, or you can skip any contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) // return all contours + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude contours inside other contours + * note: EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up areas of solid color. + * + * - turn the display of contours ON or OFF. Turning this on helps debugging but takes up valuable CPU time. + * .setDrawContours(true) + * + * - include any pre-processing of the image or mask before looking for Blobs. + * There are some extra processing you can include to improve the formation of blobs. Using these features requires + * an understanding of how they may effect the final blobs. The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) Blurring an image helps to provide a smooth color transition between objects, and smoother contours. + * The higher the number of pixels, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A "pixels" size of 5 is good for a 320x240 image. + * .setErodeSize(int pixels) Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * .setDilateSize(int pixels) Dilation makes objects more visible by filling in small holes, making lines appear thicker, + * and making filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. + */ + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)) // search central 1/4 of camera view + .setDrawContours(true) // Show contours on the Stream Preview + .setBlurSize(5) // Smooth the transitions between different colors in image + .build(); + + /* + * Build a vision portal to run the Color Locator process. + * + * - Add the colorLocator process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorLocator) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("preview on/off", "... Camera Stream\n"); + + // Read the current list + List blobs = colorLocator.getBlobs(); + + /* + * The list of Blobs can be filtered to remove unwanted Blobs. + * Note: All contours will be still displayed on the Stream Preview, but only those that satisfy the filter + * conditions will remain in the current list of "blobs". Multiple filters may be used. + * + * Use any of the following filters. + * + * ColorBlobLocatorProcessor.Util.filterByArea(minArea, maxArea, blobs); + * A Blob's area is the number of pixels contained within the Contour. Filter out any that are too big or small. + * Start with a large range and then refine the range based on the likely size of the desired object in the viewfinder. + * + * ColorBlobLocatorProcessor.Util.filterByDensity(minDensity, maxDensity, blobs); + * A blob's density is an indication of how "full" the contour is. + * If you put a rubber band around the contour you would get the "Convex Hull" of the contour. + * The density is the ratio of Contour-area to Convex Hull-area. + * + * ColorBlobLocatorProcessor.Util.filterByAspectRatio(minAspect, maxAspect, blobs); + * A blob's Aspect ratio is the ratio of boxFit long side to short side. + * A perfect Square has an aspect ratio of 1. All others are > 1 + */ + ColorBlobLocatorProcessor.Util.filterByArea(50, 20000, blobs); // filter out very small blobs. + + /* + * The list of Blobs can be sorted using the same Blob attributes as listed above. + * No more than one sort call should be made. Sorting can use ascending or descending order. + * ColorBlobLocatorProcessor.Util.sortByArea(SortOrder.DESCENDING, blobs); // Default + * ColorBlobLocatorProcessor.Util.sortByDensity(SortOrder.DESCENDING, blobs); + * ColorBlobLocatorProcessor.Util.sortByAspectRatio(SortOrder.DESCENDING, blobs); + */ + + telemetry.addLine(" Area Density Aspect Center"); + + // Display the size (area) and center location for each Blob. + for(ColorBlobLocatorProcessor.Blob b : blobs) + { + RotatedRect boxFit = b.getBoxFit(); + telemetry.addLine(String.format("%5d %4.2f %5.2f (%3d,%3d)", + b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y)); + } + + telemetry.update(); + sleep(50); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java new file mode 100644 index 00000000..33afccf1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java @@ -0,0 +1,136 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.graphics.Color; +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor; + +/* + * This OpMode illustrates how to use a video source (camera) as a color sensor + * + * A "color sensor" will typically determine the color of the object that it is pointed at. + * + * This sample performs the same function, except it uses a video camera to inspect an object or scene. + * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view. + * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching color will be selected. + * + * To perform this function, a VisionPortal runs a PredominantColorProcessor process. + * The PredominantColorProcessor process is created first, and then the VisionPortal is built to use this process. + * The PredominantColorProcessor analyses the ROI and splits the colored pixels into several color-clusters. + * The largest of these clusters is then considered to be the "Predominant Color" + * The process then matches the Predominant Color with the closest Swatch and returns that match. + * + * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest, + * The Predominant Color is used to paint the rectangle border, so the user can verify that the color is reasonable. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Sensor", group = "Concept") +public class ConceptVisionColorSensor extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class. + * + * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% width/height square centered on screen + * + * - Set the list of "acceptable" color swatches (matches). + * Only colors that you assign here will be returned. + * If you know the sensor will be pointing to one of a few specific colors, enter them here. + * Or, if the sensor may be pointed randomly, provide some additional colors that may match the surrounding. + * Possible choices are: + * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE; + * + * Note that in the example shown below, only some of the available colors are included. + * This will force any other colored region into one of these colors. + * eg: Green may be reported as YELLOW, as this may be the "closest" match. + */ + PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder() + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1)) + .setSwatches( + PredominantColorProcessor.Swatch.RED, + PredominantColorProcessor.Swatch.BLUE, + PredominantColorProcessor.Swatch.YELLOW, + PredominantColorProcessor.Swatch.BLACK, + PredominantColorProcessor.Swatch.WHITE) + .build(); + + /* + * Build a vision portal to run the Color Sensor process. + * + * - Add the colorSensor process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorSensor) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging. + + // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("DS preview on/off", "3 dots, Camera Stream\n"); + + // Request the most recent color analysis. + // This will return the closest matching colorSwatch and the predominant RGB color. + // Note: to take actions based on the detected color, simply use the colorSwatch in a comparison or switch. + // eg: + // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {... some code ...} + PredominantColorProcessor.Result result = colorSensor.getAnalysis(); + + // Display the Color Sensor result. + telemetry.addData("Best Match:", result.closestSwatch); + telemetry.addLine(String.format("R %3d, G %3d, B %3d", Color.red(result.rgb), Color.green(result.rgb), Color.blue(result.rgb))); + telemetry.update(); + + sleep(20); + } + } +} \ No newline at end of file diff --git a/Vision/src/main/java/android/graphics/Bitmap.java b/Vision/src/main/java/android/graphics/Bitmap.java index fcb2d005..8210c5f3 100644 --- a/Vision/src/main/java/android/graphics/Bitmap.java +++ b/Vision/src/main/java/android/graphics/Bitmap.java @@ -211,6 +211,10 @@ public static Bitmap createBitmap(int width, int height, Config config) { return bm; } + /** + * Internal: theBitmap represents the underlying skiko Bitmap + * This field is not present in native android.graphics + */ public final org.jetbrains.skia.Bitmap theBitmap; public Bitmap() { diff --git a/Vision/src/main/java/android/graphics/Canvas.java b/Vision/src/main/java/android/graphics/Canvas.java index 1bea8576..17f7605c 100644 --- a/Vision/src/main/java/android/graphics/Canvas.java +++ b/Vision/src/main/java/android/graphics/Canvas.java @@ -28,6 +28,10 @@ public class Canvas { + /** + * Internal: theCanvas represents the underlying skiko Canvas + * This field is not present in native android.graphics + */ public final org.jetbrains.skia.Canvas theCanvas; private Bitmap backingBitmap = null; diff --git a/Vision/src/main/java/android/graphics/Color.java b/Vision/src/main/java/android/graphics/Color.java index 7460ec3e..02243cc7 100644 --- a/Vision/src/main/java/android/graphics/Color.java +++ b/Vision/src/main/java/android/graphics/Color.java @@ -1383,9 +1383,77 @@ public static int HSVToColor(@IntRange(from = 0, to = 255) int alpha, @Size(3) f } return nativeHSVToColor(alpha, hsv); } - private static native void nativeRGBToHSV(int red, int greed, int blue, float hsv[]); - private static native int nativeHSVToColor(int alpha, float hsv[]); + private static void nativeRGBToHSV(int red, int green, int blue, float hsv[]) { + int min = Math.min(red, Math.min(green, blue)); + int max = Math.max(red, Math.max(green, blue)); + int delta = max - min; + + float v = (float) max / 255; + if (v < 0 || v > 1.0f) + throw new RuntimeException("RGB max is out of range"); + + if (delta == 0) { // we're a shade of gray + hsv[0] = 0; + hsv[1] = 0; + hsv[2] = v; + return; + } + + float s = (float) delta / max; + if (s < 0 || s > 1.0f) + throw new RuntimeException("RGB delta is out of range"); + + float h; + if (red == max) + h = (float) (green - blue) / delta; + else if (green == max) + h = 2.0f + (float) (blue - red) / delta; + else // blue == max + h = 4.0f + (float) (red - green) / delta; + + h *= 60; + if (h < 0) + h += 360.0f; + + if (h < 0 || h >= 360.0f) + throw new RuntimeException("h is out of range"); + + hsv[0] = h; + hsv[1] = s; + hsv[2] = v; + } + + private static int nativeHSVToColor(int alpha, float hsv[]) { + int s = hsv[1] < 0 ? 0 : hsv[1] >= 1.0f ? 255 : ((int) (hsv[1] * (1 << 16))) >> 8; + int v = hsv[2] < 0 ? 0 : hsv[2] >= 1.0f ? 255 : ((int) (hsv[2] * (1 << 16))) >> 8; + + if (s == 0) // shade of gray + return (alpha << 24) | (v << 16) | (v << 8) | v; + int hx = (hsv[0] < 0 || hsv[0] >= 360.0f) ? 0 : ((int) (hsv[0]/60 * (1 << 16))); + int f = hx & 0xFFFF; + + int v_scale = v+1; + int p = ((255 - s) * v_scale) >> 8; + int q = ((255 - (s * f >> 16)) * v_scale) >> 8; + int t = ((255 - (s * (1 << 16 - f) >> 16)) * v_scale) >> 8; + + int r, g, b; + + if (hx >> 16 >= 6) + throw new RuntimeException("hx is out of range"); + switch (hx >> 16) { + case 0: r = v; g = t; b = p; break; + case 1: r = q; g = v; b = p; break; + case 2: r = p; g = v; b = t; break; + case 3: r = p; g = q; b = v; break; + case 4: r = t; g = p; b = v; break; + default: r = v; g = p; b = q; break; + } + return (alpha << 24) | (r << 16) | (g << 8) | b; + } + private static final HashMap sColorNameMap; + static { sColorNameMap = new HashMap<>(); sColorNameMap.put("black", BLACK); diff --git a/Vision/src/main/java/android/graphics/FontCache.java b/Vision/src/main/java/android/graphics/FontCache.java index 052c090c..71bd9256 100644 --- a/Vision/src/main/java/android/graphics/FontCache.java +++ b/Vision/src/main/java/android/graphics/FontCache.java @@ -27,19 +27,22 @@ import java.util.HashMap; +/** + * A cache for fonts to avoid creating the same font multiple times. + */ class FontCache { private static HashMap> cache = new HashMap<>(); - public static Font makeFont(Typeface theTypeface, float textSize) { - if(!cache.containsKey(theTypeface)) { - cache.put(theTypeface, new HashMap<>()); + static Font makeFont(Typeface typeface, float textSize) { + if(!cache.containsKey(typeface)) { + cache.put(typeface, new HashMap<>()); } - HashMap sizeCache = cache.get(theTypeface); + HashMap sizeCache = cache.get(typeface); if(!sizeCache.containsKey((int) (textSize * 1000))) { - sizeCache.put((int) (textSize * 1000), new Font(theTypeface.theTypeface, textSize)); + sizeCache.put((int) (textSize * 1000), new Font(typeface.theTypeface, textSize)); } return sizeCache.get((int) (textSize * 1000)); diff --git a/Vision/src/main/java/android/graphics/Paint.java b/Vision/src/main/java/android/graphics/Paint.java index 2abbccb1..b6417cda 100644 --- a/Vision/src/main/java/android/graphics/Paint.java +++ b/Vision/src/main/java/android/graphics/Paint.java @@ -176,6 +176,10 @@ public static class FontMetrics { public float leading; } + /** + * Internal: thePaint represents the underlying skiko paint + * This field is not present in native android.graphics + */ public org.jetbrains.skia.Paint thePaint; private Typeface typeface; @@ -354,7 +358,7 @@ public float getStrokeMiter() { public Typeface getTypeface() { if(typeface == null) { - typeface = Typeface.DEFAULT; + return Typeface.DEFAULT; } return typeface; diff --git a/Vision/src/main/java/android/graphics/Path.java b/Vision/src/main/java/android/graphics/Path.java index 99abbcdd..178d7f90 100644 --- a/Vision/src/main/java/android/graphics/Path.java +++ b/Vision/src/main/java/android/graphics/Path.java @@ -60,6 +60,10 @@ public enum Direction { final int nativeInt; } + /** + * Internal: thePath represents the underlying skiko Path + * This field is not present in native android.graphics + */ public org.jetbrains.skia.Path thePath; public Path() { diff --git a/Vision/src/main/java/android/graphics/TemporaryBuffer.java b/Vision/src/main/java/android/graphics/TemporaryBuffer.java new file mode 100644 index 00000000..b98e3d9b --- /dev/null +++ b/Vision/src/main/java/android/graphics/TemporaryBuffer.java @@ -0,0 +1,27 @@ +package android.graphics; + +import com.android.internal.util.ArrayUtils; + +/** + * @hide + */ +public class TemporaryBuffer { + public static char[] obtain(int len) { + char[] buf; + synchronized (TemporaryBuffer.class) { + buf = sTemp; + sTemp = null; + } + if (buf == null || buf.length < len) { + buf = new char[ArrayUtils.idealCharArraySize(len)]; + } + return buf; + } + public static void recycle(char[] temp) { + if (temp.length > 1000) return; + synchronized (TemporaryBuffer.class) { + sTemp = temp; + } + } + private static char[] sTemp = null; +} \ No newline at end of file diff --git a/Vision/src/main/java/android/graphics/Typeface.java b/Vision/src/main/java/android/graphics/Typeface.java index b6967f6e..9a56291e 100644 --- a/Vision/src/main/java/android/graphics/Typeface.java +++ b/Vision/src/main/java/android/graphics/Typeface.java @@ -23,15 +23,23 @@ package android.graphics; +import org.jetbrains.skia.Data; import org.jetbrains.skia.FontMgr; import org.jetbrains.skia.FontStyle; +import java.io.FileNotFoundException; +import java.io.IOException; + public class Typeface { - public static Typeface DEFAULT = new Typeface(FontMgr.Companion.getDefault().matchFamilyStyle(null, FontStyle.Companion.getNORMAL())); - public static Typeface DEFAULT_BOLD = new Typeface(FontMgr.Companion.getDefault().matchFamilyStyle(null, FontStyle.Companion.getBOLD())); - public static Typeface DEFAULT_ITALIC = new Typeface(FontMgr.Companion.getDefault().matchFamilyStyle(null, FontStyle.Companion.getITALIC())); + public static Typeface DEFAULT = new Typeface(FontMgr.Companion.getDefault().makeFromData(loadDataFromResource("/fonts/Roboto-Regular.ttf"), 0)); + public static Typeface DEFAULT_BOLD = new Typeface(FontMgr.Companion.getDefault().makeFromData(loadDataFromResource("/fonts/Roboto-Bold.ttf"), 0)); + public static Typeface DEFAULT_ITALIC = new Typeface(FontMgr.Companion.getDefault().makeFromData(loadDataFromResource("/fonts/Roboto-Italic.ttf"), 0)); + /** + * Internal: theTypeface represents the underlying skiko Typeface + * This field is not present in native android.graphics + */ public org.jetbrains.skia.Typeface theTypeface; public Typeface(long ptr) { @@ -51,4 +59,17 @@ public Rect getBounds() { ); } + private static Data loadDataFromResource(String resource) { + try { + byte[] bytes = Typeface.class.getResourceAsStream(resource).readAllBytes(); + + return Data.Companion.makeFromBytes( + bytes, + 0, bytes.length + ); + } catch (IOException e) { + throw new IllegalArgumentException("Failed to load from resource: " + resource, e); + } + } + } diff --git a/Vision/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpMode.java b/Vision/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpMode.java index 779bfe32..ec861393 100644 --- a/Vision/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpMode.java +++ b/Vision/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpMode.java @@ -31,6 +31,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.robotcore.eventloop.opmode; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import io.github.deltacv.vision.external.util.FrameQueue; import io.github.deltacv.vision.internal.opmode.OpModeNotification; @@ -53,6 +54,10 @@ public abstract class OpMode extends TimestampedOpenCvPipeline { // never in my volatile boolean isStarted = false; volatile boolean stopRequested = false; + // Stubs! + public Gamepad gamepad1 = new Gamepad(); + public Gamepad gamepad2 = new Gamepad(); + protected FrameQueue inputQueue; public HardwareMap hardwareMap; diff --git a/Vision/src/main/java/com/qualcomm/robotcore/hardware/Gamepad.java b/Vision/src/main/java/com/qualcomm/robotcore/hardware/Gamepad.java new file mode 100644 index 00000000..bfef1dab --- /dev/null +++ b/Vision/src/main/java/com/qualcomm/robotcore/hardware/Gamepad.java @@ -0,0 +1,424 @@ +/* + * Copyright (c) 2014, 2015 Qualcomm Technologies Inc + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * (subject to the limitations in the disclaimer below) provided that the following conditions are + * met: + * + * Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions + * and the following disclaimer in the documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Qualcomm Technologies Inc nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS + * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package com.qualcomm.robotcore.hardware; + +/** + * Monitor a hardware gamepad. In the case of EOCV-Sim, this is a stub class that does nothing. + *

+ * The buttons, analog sticks, and triggers are represented a public + * member variables that can be read from or written to directly. + *

+ * Analog sticks are represented as floats that range from -1.0 to +1.0. They will be 0.0 while at + * rest. The horizontal axis is labeled x, and the vertical axis is labeled y. + *

+ * Triggers are represented as floats that range from 0.0 to 1.0. They will be at 0.0 while at + * rest. + *

+ * Buttons are boolean values. They will be true if the button is pressed, otherwise they will be + * false. + *

+ * The codes KEYCODE_BUTTON_SELECT and KEYCODE_BACK are both be handled as a "back" button event. + * Older Android devices (Kit Kat) map a Logitech F310 "back" button press to a KEYCODE_BUTTON_SELECT event. + * Newer Android devices (Marshmallow or greater) map this "back" button press to a KEYCODE_BACK event. + * Also, the REV Robotics Gamepad (REV-31-1159) has a "select" button instead of a "back" button on the gamepad. + *

+ * The dpad is represented as 4 buttons, dpad_up, dpad_down, dpad_left, and dpad_right + */ +@SuppressWarnings("unused") +public class Gamepad { + + /** + * A gamepad with an ID equal to ID_UNASSOCIATED has not been associated with any device. + */ + public static final int ID_UNASSOCIATED = -1; + + /** + * A gamepad with a phantom id a synthetic one made up by the system + */ + public static final int ID_SYNTHETIC = -2; + + public enum Type { + // Do NOT change the order/names of existing entries, + // you will break backwards compatibility!! + UNKNOWN(LegacyType.UNKNOWN), + LOGITECH_F310(LegacyType.LOGITECH_F310), + XBOX_360(LegacyType.XBOX_360), + SONY_PS4(LegacyType.SONY_PS4), // This indicates a PS4-compatible controller that is being used through our compatibility mode + SONY_PS4_SUPPORTED_BY_KERNEL(LegacyType.SONY_PS4); // This indicates a PS4-compatible controller that is being used through the DualShock 4 Linux kernel driver. + + private final LegacyType correspondingLegacyType; + Type(LegacyType correspondingLegacyType) { + this.correspondingLegacyType = correspondingLegacyType; + } + } + + // LegacyType is necessary because robocol gamepad version 3 was written in a way that was not + // forwards-compatible, so we have to keep sending V3-compatible values. + public enum LegacyType { + // Do NOT change the order or names of existing entries, or add new entries. + // You will break backwards compatibility!! + UNKNOWN, + LOGITECH_F310, + XBOX_360, + SONY_PS4 + } + + @SuppressWarnings("UnusedAssignment") + public volatile Type type = Type.UNKNOWN; // IntelliJ thinks this is redundant, but it is NOT. Must be a bug in the analyzer? + + /** + * left analog stick horizontal axis + */ + public volatile float left_stick_x = 0f; + + /** + * left analog stick vertical axis + */ + public volatile float left_stick_y = 0f; + + /** + * right analog stick horizontal axis + */ + public volatile float right_stick_x = 0f; + + /** + * right analog stick vertical axis + */ + public volatile float right_stick_y = 0f; + + /** + * dpad up + */ + public volatile boolean dpad_up = false; + + /** + * dpad down + */ + public volatile boolean dpad_down = false; + + /** + * dpad left + */ + public volatile boolean dpad_left = false; + + /** + * dpad right + */ + public volatile boolean dpad_right = false; + + /** + * button a + */ + public volatile boolean a = false; + + /** + * button b + */ + public volatile boolean b = false; + + /** + * button x + */ + public volatile boolean x = false; + + /** + * button y + */ + public volatile boolean y = false; + + /** + * button guide - often the large button in the middle of the controller. The OS may + * capture this button before it is sent to the app; in which case you'll never + * receive it. + */ + public volatile boolean guide = false; + + /** + * button start + */ + public volatile boolean start = false; + + /** + * button back + */ + public volatile boolean back = false; + + /** + * button left bumper + */ + public volatile boolean left_bumper = false; + + /** + * button right bumper + */ + public volatile boolean right_bumper = false; + + /** + * left stick button + */ + public volatile boolean left_stick_button = false; + + /** + * right stick button + */ + public volatile boolean right_stick_button = false; + + /** + * left trigger + */ + public volatile float left_trigger = 0f; + + /** + * right trigger + */ + public volatile float right_trigger = 0f; + + /** + * PS4 Support - Circle + */ + public volatile boolean circle = false; + + /** + * PS4 Support - cross + */ + public volatile boolean cross = false; + + /** + * PS4 Support - triangle + */ + public volatile boolean triangle = false; + + /** + * PS4 Support - square + */ + public volatile boolean square = false; + + /** + * PS4 Support - share + */ + public volatile boolean share = false; + + /** + * PS4 Support - options + */ + public volatile boolean options = false; + + /** + * PS4 Support - touchpad + */ + public volatile boolean touchpad = false; + public volatile boolean touchpad_finger_1; + public volatile boolean touchpad_finger_2; + public volatile float touchpad_finger_1_x; + public volatile float touchpad_finger_1_y; + public volatile float touchpad_finger_2_x; + public volatile float touchpad_finger_2_y; + + /** + * PS4 Support - PS Button + */ + public volatile boolean ps = false; + + /** + * ID assigned to this gamepad by the OS. This value can change each time the device is plugged in. + */ + public volatile int id = ID_UNASSOCIATED; // public only for historical reasons + + public void setGamepadId(int id) { + this.id = id; + } + public int getGamepadId() { + return this.id; + } + + /** + * Relative timestamp of the last time an event was detected + */ + public volatile long timestamp = 0; + + public Gamepad() { + this.type = type(); + } + + /** + * Reset this gamepad into its initial state + */ + public void reset() { + left_stick_x = 0f; + left_stick_y = 0f; + right_stick_x = 0f; + right_stick_y = 0f; + dpad_up = false; + dpad_down = false; + dpad_left = false; + dpad_right = false; + a = false; + b = false; + x = false; + y = false; + guide = false; + start = false; + back = false; + left_bumper = false; + right_bumper = false; + left_stick_button = false; + right_stick_button = false; + left_trigger = 0f; + right_trigger = 0f; + circle = false; + cross = false; + triangle = false; + square = false; + share = false; + options = false; + touchpad = false; + touchpad_finger_1 = false; + touchpad_finger_2 = false; + touchpad_finger_1_x = 0f; + touchpad_finger_1_y = 0f; + touchpad_finger_2_x = 0f; + touchpad_finger_2_y = 0f; + ps = false; + timestamp = 0; + } + + /** + * Are all analog sticks and triggers in their rest position? + * @return true if all analog sticks and triggers are at rest; otherwise false + */ + public boolean atRest() { + return ( + left_stick_x == 0f && left_stick_y == 0f && + right_stick_x == 0f && right_stick_y == 0f && + left_trigger == 0f && right_trigger == 0f); + } + + /** + * Get the type of gamepad as a {@link Type}. This method defaults to "UNKNOWN". + * @return gamepad type + */ + public Type type() { + return type; + } + + /** + * Get the type of gamepad as a {@link LegacyType}. This method defaults to "UNKNOWN". + * @return gamepad type + */ + private LegacyType legacyType() { + return type.correspondingLegacyType; + } + + + /** + * Display a summary of this gamepad, including the state of all buttons, analog sticks, and triggers + * @return a summary + */ + @Override + public String toString() { + + switch (type) { + case SONY_PS4: + case SONY_PS4_SUPPORTED_BY_KERNEL: + return ps4ToString(); + + case UNKNOWN: + case LOGITECH_F310: + case XBOX_360: + default: + return genericToString(); + } + } + + + protected String ps4ToString() { + String buttons = new String(); + if (dpad_up) buttons += "dpad_up "; + if (dpad_down) buttons += "dpad_down "; + if (dpad_left) buttons += "dpad_left "; + if (dpad_right) buttons += "dpad_right "; + if (cross) buttons += "cross "; + if (circle) buttons += "circle "; + if (square) buttons += "square "; + if (triangle) buttons += "triangle "; + if (ps) buttons += "ps "; + if (share) buttons += "share "; + if (options) buttons += "options "; + if (touchpad) buttons += "touchpad "; + if (left_bumper) buttons += "left_bumper "; + if (right_bumper) buttons += "right_bumper "; + if (left_stick_button) buttons += "left stick button "; + if (right_stick_button) buttons += "right stick button "; + + return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s", + id, 0, left_stick_x, left_stick_y, + right_stick_x, right_stick_y, left_trigger, right_trigger, buttons); + } + + protected String genericToString() { + String buttons = new String(); + if (dpad_up) buttons += "dpad_up "; + if (dpad_down) buttons += "dpad_down "; + if (dpad_left) buttons += "dpad_left "; + if (dpad_right) buttons += "dpad_right "; + if (a) buttons += "a "; + if (b) buttons += "b "; + if (x) buttons += "x "; + if (y) buttons += "y "; + if (guide) buttons += "guide "; + if (start) buttons += "start "; + if (back) buttons += "back "; + if (left_bumper) buttons += "left_bumper "; + if (right_bumper) buttons += "right_bumper "; + if (left_stick_button) buttons += "left stick button "; + if (right_stick_button) buttons += "right stick button "; + + return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s", + id, 0, left_stick_x, left_stick_y, + right_stick_x, right_stick_y, left_trigger, right_trigger, buttons); + } + + /** + * Alias buttons so that XBOX & PS4 native button labels can be used in use code. + * Should allow a team to program with whatever controllers they prefer, but + * be able to swap controllers easily without changing code. + */ + protected void updateButtonAliases(){ + // There is no assignment for touchpad because there is no equivalent on XBOX controllers. + circle = b; + cross = a; + triangle = y; + square = x; + share = back; + options = start; + ps = guide; + } +} diff --git a/Vision/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java b/Vision/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java index f0c39b30..1a58f1a0 100644 --- a/Vision/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java +++ b/Vision/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java @@ -21,7 +21,7 @@ public T get(Class classType, String deviceName) { return (T) new SourcedCameraNameImpl(ThreadSourceHander.hand(deviceName)); } - return null; + throw new IllegalArgumentException("Unknown device type " + classType.getName()); } } \ No newline at end of file diff --git a/Vision/src/main/java/io/github/deltacv/vision/external/gui/SwingOpenCvViewport.kt b/Vision/src/main/java/io/github/deltacv/vision/external/gui/SwingOpenCvViewport.kt index 8bfc0763..300e15af 100644 --- a/Vision/src/main/java/io/github/deltacv/vision/external/gui/SwingOpenCvViewport.kt +++ b/Vision/src/main/java/io/github/deltacv/vision/external/gui/SwingOpenCvViewport.kt @@ -27,9 +27,9 @@ import android.graphics.Canvas import io.github.deltacv.common.image.MatPoster import org.firstinspires.ftc.robotcore.internal.collections.EvictingBlockingQueue import org.jetbrains.skia.Color -import org.jetbrains.skiko.GenericSkikoView import org.jetbrains.skiko.SkiaLayer -import org.jetbrains.skiko.SkikoView +import org.jetbrains.skiko.SkiaLayerRenderDelegate +import org.jetbrains.skiko.SkikoRenderDelegate import org.opencv.android.Utils import org.opencv.core.Mat import org.opencv.core.Size @@ -44,7 +44,6 @@ import java.util.concurrent.ArrayBlockingQueue import java.util.concurrent.TimeUnit import javax.swing.JComponent import javax.swing.SwingUtilities -import kotlin.jvm.Throws class SwingOpenCvViewport(size: Size, fpsMeterDescriptor: String = "deltacv Vision") : OpenCvViewport, MatPoster { @@ -94,7 +93,7 @@ class SwingOpenCvViewport(size: Size, fpsMeterDescriptor: String = "deltacv Visi framebufferRecycler!!.returnMat(value) } - skiaLayer.skikoView = GenericSkikoView(skiaLayer, object: SkikoView { + skiaLayer.renderDelegate = SkiaLayerRenderDelegate(skiaLayer, object: SkikoRenderDelegate { override fun onRender(canvas: org.jetbrains.skia.Canvas, width: Int, height: Int, nanoTime: Long) { renderCanvas(Canvas(canvas, width, height)) diff --git a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagCanvasAnnotator.java b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagCanvasAnnotator.java index 484aca1e..8e159b15 100644 --- a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagCanvasAnnotator.java +++ b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagCanvasAnnotator.java @@ -36,8 +36,8 @@ import android.graphics.Canvas; import android.graphics.Color; import android.graphics.Paint; - import android.graphics.Typeface; + import org.opencv.calib3d.Calib3d; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; @@ -117,9 +117,9 @@ void drawAxisMarker(AprilTagDetection detection, Canvas canvas, double tagsize) // The origin of the coordinate space is assumed to be in the center of the detection. MatOfPoint3f axis = new MatOfPoint3f( new Point3(0,0,0), - new Point3(-axisLength,0,0), - new Point3(0,-axisLength,0), - new Point3(0,0,-axisLength) + new Point3(axisLength,0,0), + new Point3(0,axisLength,0), + new Point3(0,0,axisLength) ); // Project those points onto the image @@ -267,7 +267,11 @@ void drawTagID(AprilTagDetection detection, Canvas canvas) { float cornerRound = 5 * canvasDensityScale; - float tag_id_width = 120*canvasDensityScale; + String text = String.format("ID %d", detection.id); + + // Implementing measureText is a bit of a pain, so we'll just leave it out for now ... + // (EOCV-Sim doesn't support measureText) + float tag_id_width = /*textPaint.measureText(text) +*/ (text.length() * textPaint.getTextSize() * 0.5f)+ 20*canvasDensityScale; float tag_id_height = 50*canvasDensityScale; float id_x = (float) detection.center.x * bmpPxToCanvasPx - tag_id_width/2; @@ -283,7 +287,7 @@ void drawTagID(AprilTagDetection detection, Canvas canvas) canvas.rotate((float) Math.toDegrees(Math.atan2(lowerRight.y - lowerLeft.y, lowerRight.x-lowerLeft.x)), (float) detection.center.x*bmpPxToCanvasPx, (float) detection.center.y*bmpPxToCanvasPx); canvas.drawRoundRect(id_x, id_y, id_x+tag_id_width, id_y+tag_id_height, cornerRound, cornerRound, rectPaint); - canvas.drawText(String.format("ID %02d", detection.id), tag_id_text_x, tag_id_text_y, textPaint); + canvas.drawText(text, tag_id_text_x, tag_id_text_y, textPaint); canvas.restore(); } diff --git a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagDetection.java b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagDetection.java index 251fe4ee..3f0bbb8c 100644 --- a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagDetection.java +++ b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagDetection.java @@ -33,6 +33,7 @@ package org.firstinspires.ftc.vision.apriltag; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.opencv.core.Point; public class AprilTagDetection @@ -78,13 +79,18 @@ public class AprilTagDetection */ public final AprilTagPoseRaw rawPose; + /* + * Robot pose data returned by the pose solver + */ + public final Pose3D robotPose; + /* * Timestamp of when the image in which this detection was found was acquired */ public final long frameAcquisitionNanoTime; public AprilTagDetection(int id, int hamming, float decisionMargin, Point center, Point[] corners, - AprilTagMetadata metadata, AprilTagPoseFtc ftcPose, AprilTagPoseRaw rawPose, long frameAcquisitionNanoTime) + AprilTagMetadata metadata, AprilTagPoseFtc ftcPose, AprilTagPoseRaw rawPose, Pose3D robotPose, long frameAcquisitionNanoTime) { this.id = id; this.hamming = hamming; @@ -94,6 +100,7 @@ public AprilTagDetection(int id, int hamming, float decisionMargin, Point center this.metadata = metadata; this.ftcPose = ftcPose; this.rawPose = rawPose; + this.robotPose = robotPose; this.frameAcquisitionNanoTime = frameAcquisitionNanoTime; } -} \ No newline at end of file +} diff --git a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessor.java b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessor.java index a7a0737f..9ed97a89 100644 --- a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessor.java +++ b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessor.java @@ -33,8 +33,14 @@ package org.firstinspires.ftc.vision.apriltag; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.vision.VisionProcessor; import org.opencv.calib3d.Calib3d; import org.openftc.apriltag.AprilTagDetectorJNI; @@ -67,18 +73,35 @@ public static AprilTagProcessor easyCreateWithDefaults() public static class Builder { + private Position cameraPosition = new Position(); + private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, 0, 0, 0, 0); private double fx, fy, cx, cy; private TagFamily tagFamily = TagFamily.TAG_36h11; private AprilTagLibrary tagLibrary = AprilTagGameDatabase.getCurrentGameTagLibrary(); private DistanceUnit outputUnitsLength = DistanceUnit.INCH; private AngleUnit outputUnitsAngle = AngleUnit.DEGREES; private int threads = THREADS_DEFAULT; + private boolean suppressCalibrationWarnings; private boolean drawAxes = false; private boolean drawCube = false; private boolean drawOutline = true; private boolean drawTagId = true; + /** + * Set the camera pose relative to the robot origin. + * @param position Position of camera relative to the robot origin + * @param orientation Orientation of camera relative to the robot origin + * @return the {@link Builder} object, to allow for method chaining + */ + public Builder setCameraPose(Position position, YawPitchRollAngles orientation) + { + cameraPosition = position; + cameraOrientation = orientation; + + return this; + } + /** * Set the camera calibration parameters (needed for accurate 6DOF pose unless the * SDK has a built in calibration for your camera) @@ -97,6 +120,17 @@ public Builder setLensIntrinsics(double fx, double fy, double cx, double cy) return this; } + /** + * Set whether any warnings about camera calibration should be suppressed + * @param suppressCalibrationWarnings whether to suppress calibration warnings + * @return the {@link Builder} object, to allow for method chaining + */ + public Builder setSuppressCalibrationWarnings(boolean suppressCalibrationWarnings) + { + this.suppressCalibrationWarnings = suppressCalibrationWarnings; + return this; + } + /** * Set the tag family this detector will be used to detect (it can only be used * for one tag family at a time) @@ -193,8 +227,8 @@ public Builder setNumThreads(int threads) /** * Create a {@link VisionProcessor} object which may be attached to - * a {link org.firstinspires.ftc.vision.VisionPortal} using - * {link org.firstinspires.ftc.vision.VisionPortal.Builder#addProcessor(VisionProcessor)} + * a {@link org.firstinspires.ftc.vision.VisionPortal} using + * {@link org.firstinspires.ftc.vision.VisionPortal.Builder#addProcessor(VisionProcessor)} * @return a {@link VisionProcessor} object */ public AprilTagProcessor build() @@ -209,11 +243,27 @@ public AprilTagProcessor build() throw new RuntimeException("Cannot create AprilTagProcessor without setting tag family!"); } + OpenGLMatrix cameraRotationMatrix = new Orientation( + AxesReference.INTRINSIC, AxesOrder.ZXZ, AngleUnit.DEGREES, + (float) cameraOrientation.getYaw(AngleUnit.DEGREES), + (float) cameraOrientation.getPitch(AngleUnit.DEGREES), + (float) cameraOrientation.getRoll(AngleUnit.DEGREES), + cameraOrientation.getAcquisitionTime()) + .getRotationMatrix(); + + OpenGLMatrix robotInCameraFrame = OpenGLMatrix.identityMatrix() + .translated( + (float) cameraPosition.toUnit(DistanceUnit.INCH).x, + (float) cameraPosition.toUnit(DistanceUnit.INCH).y, + (float) cameraPosition.toUnit(DistanceUnit.INCH).z) + .multiplied(cameraRotationMatrix) + .inverted(); + return new AprilTagProcessorImpl( - fx, fy, cx, cy, + robotInCameraFrame, fx, fy, cx, cy, outputUnitsLength, outputUnitsAngle, tagLibrary, drawAxes, drawCube, drawOutline, drawTagId, - tagFamily, threads + tagFamily, threads, suppressCalibrationWarnings ); } } diff --git a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessorImpl.java b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessorImpl.java index ccd58a8f..023284ed 100644 --- a/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessorImpl.java +++ b/Vision/src/main/java/org/firstinspires/ftc/vision/apriltag/AprilTagProcessorImpl.java @@ -35,16 +35,20 @@ import android.graphics.Canvas; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.util.MovingStatistics; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.matrices.GeneralMatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; import org.opencv.calib3d.Calib3d; import org.opencv.core.CvType; @@ -57,16 +61,16 @@ import org.opencv.imgproc.Imgproc; import org.openftc.apriltag.AprilTagDetectorJNI; import org.openftc.apriltag.ApriltagDetectionJNI; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.ArrayList; -import java.util.logging.Logger; -@Disabled public class AprilTagProcessorImpl extends AprilTagProcessor { public static final String TAG = "AprilTagProcessorImpl"; - private Logger logger = Logger.getLogger(TAG); + Logger logger = LoggerFactory.getLogger(TAG); private long nativeApriltagPtr; private Mat grey = new Mat(); @@ -85,6 +89,7 @@ public class AprilTagProcessorImpl extends AprilTagProcessor private double fy; private double cx; private double cy; + private final boolean suppressCalibrationWarnings; private final AprilTagLibrary tagLibrary; @@ -97,10 +102,15 @@ public class AprilTagProcessorImpl extends AprilTagProcessor private final DistanceUnit outputUnitsLength; private final AngleUnit outputUnitsAngle; - private volatile PoseSolver poseSolver = PoseSolver.OPENCV_ITERATIVE; + private volatile PoseSolver poseSolver = PoseSolver.APRILTAG_BUILTIN; - public AprilTagProcessorImpl(double fx, double fy, double cx, double cy, DistanceUnit outputUnitsLength, AngleUnit outputUnitsAngle, AprilTagLibrary tagLibrary, boolean drawAxes, boolean drawCube, boolean drawOutline, boolean drawTagID, TagFamily tagFamily, int threads) + private OpenGLMatrix robotInCameraFrame; + + public AprilTagProcessorImpl(OpenGLMatrix robotInCameraFrame, double fx, double fy, double cx, double cy, DistanceUnit outputUnitsLength, AngleUnit outputUnitsAngle, AprilTagLibrary tagLibrary, + boolean drawAxes, boolean drawCube, boolean drawOutline, boolean drawTagID, TagFamily tagFamily, int threads, boolean suppressCalibrationWarnings) { + this.robotInCameraFrame = robotInCameraFrame; + this.fx = fx; this.fy = fy; this.cx = cx; @@ -109,6 +119,7 @@ public AprilTagProcessorImpl(double fx, double fy, double cx, double cy, Distanc this.tagLibrary = tagLibrary; this.outputUnitsLength = outputUnitsLength; this.outputUnitsAngle = outputUnitsAngle; + this.suppressCalibrationWarnings = suppressCalibrationWarnings; this.drawAxes = drawAxes; this.drawCube = drawCube; this.drawOutline = drawOutline; @@ -137,39 +148,84 @@ protected void finalize() @Override public void init(int width, int height, CameraCalibration calibration) { - // If the user didn't give us a calibration, but we have one built in, - // then go ahead and use it!! - if (calibration != null && fx == 0 && fy == 0 && cx == 0 && cy == 0 - && !(calibration.focalLengthX == 0 && calibration.focalLengthY == 0 && calibration.principalPointX == 0 && calibration.principalPointY == 0)) // needed because we may get an all zero calibration to indicate none, instead of null + // ATTEMPT 1 - If the user provided their own calibration, use that + if (fx != 0 && fy != 0 && cx != 0 && cy != 0) + { + logger.debug(String.format("User provided their own camera calibration fx=%7.3f fy=%7.3f cx=%7.3f cy=%7.3f", + fx, fy, cx, cy)); + } + + // ATTEMPT 2 - If we have valid calibration we can use, use it + else if (calibration != null && !calibration.isDegenerate()) // needed because we may get an all zero calibration to indicate none, instead of null { fx = calibration.focalLengthX; fy = calibration.focalLengthY; cx = calibration.principalPointX; cy = calibration.principalPointY; - logger.info(String.format("User did not provide a camera calibration; but we DO have a built in calibration we can use.\n [%dx%d] (may be scaled) %s\nfx=%7.3f fy=%7.3f cx=%7.3f cy=%7.3f", - calibration.getSize().getWidth(), calibration.getSize().getHeight(), calibration.getIdentity().toString(), fx, fy, cx, cy)); + // Note that this might have been a scaled calibration - inform the user if so + if (calibration.resolutionScaledFrom != null) + { + String msg = String.format("Camera has not been calibrated for [%dx%d]; applying a scaled calibration from [%dx%d].", width, height, calibration.resolutionScaledFrom.getWidth(), calibration.resolutionScaledFrom.getHeight()); + + if (!suppressCalibrationWarnings) + { + logger.warn(msg); + } + } + // Nope, it was a full up proper calibration - no need to pester the user about anything + else + { + logger.debug(String.format("User did not provide a camera calibration; but we DO have a built in calibration we can use.\n [%dx%d] (NOT scaled) %s\nfx=%7.3f fy=%7.3f cx=%7.3f cy=%7.3f", + calibration.getSize().getWidth(), calibration.getSize().getHeight(), calibration.getIdentity().toString(), fx, fy, cx, cy)); + } } - else if (fx == 0 && fy == 0 && cx == 0 && cy == 0) + + // Okay, we aren't going to have any calibration data we can use, but there are 2 cases to check + else { - // set it to *something* so we don't crash the native code + // NO-OP, we cannot implement this for EOCV-Sim in the same way as the FTC SDK + + /* + // If we have a calibration on file, but with a wrong aspect ratio, + // we can't use it, but hey at least we can let the user know about it. + if (calibration instanceof PlaceholderCalibratedAspectRatioMismatch) + { + StringBuilder supportedResBuilder = new StringBuilder(); + + for (CameraCalibration cal : CameraCalibrationHelper.getInstance().getCalibrations(calibration.getIdentity())) + { + supportedResBuilder.append(String.format("[%dx%d],", cal.getSize().getWidth(), cal.getSize().getHeight())); + } - String warning = "User did not provide a camera calibration, nor was a built-in calibration found for this camera; 6DOF pose data will likely be inaccurate."; - logger.warning(warning); + String msg = String.format("Camera has not been calibrated for [%dx%d]. Pose estimates will likely be inaccurate. However, there are built in calibrations for resolutions: %s", + width, height, supportedResBuilder.toString()); + if (!suppressCalibrationWarnings) + { + logger.warn(msg); + } + + + // Nah, we got absolutely nothing + else*/ + { + String warning = "User did not provide a camera calibration, nor was a built-in calibration found for this camera. Pose estimates will likely be inaccurate."; + + if (!suppressCalibrationWarnings) + { + logger.warn(warning); + } + } + + // IN EITHER CASE, set it to *something* so we don't crash the native code fx = 578.272; fy = 578.272; cx = width/2; cy = height/2; } - else - { - logger.info(String.format("User provided their own camera calibration fx=%7.3f fy=%7.3f cx=%7.3f cy=%7.3f", - fx, fy, cx, cy)); - } constructMatrix(); - canvasAnnotator = new AprilTagCanvasAnnotator(cameraMatrix); } @@ -225,6 +281,7 @@ ArrayList runAprilTagDetectorForMultipleTagSizes(long capture AprilTagPoseRaw rawPose; AprilTagPoseFtc ftcPose; + Pose3D robotPose; if (metadata != null) { @@ -294,10 +351,13 @@ ArrayList runAprilTagDetectorForMultipleTagSizes(long capture Math.hypot(rawPose.x, rawPose.z), // range outputUnitsAngle.fromUnit(AngleUnit.RADIANS, Math.atan2(-rawPose.x, rawPose.z)), // bearing outputUnitsAngle.fromUnit(AngleUnit.RADIANS, Math.atan2(-rawPose.y, rawPose.z))); // elevation + + robotPose = computeRobotPose(rawPose, metadata, captureTimeNanos); } else { ftcPose = null; + robotPose = null; } double[] center = ApriltagDetectionJNI.getCenterpoint(ptrDetection); @@ -306,7 +366,7 @@ ArrayList runAprilTagDetectorForMultipleTagSizes(long capture ApriltagDetectionJNI.getId(ptrDetection), ApriltagDetectionJNI.getHamming(ptrDetection), ApriltagDetectionJNI.getDecisionMargin(ptrDetection), - new Point(center[0], center[1]), cornerPts, metadata, ftcPose, rawPose, captureTimeNanos)); + new Point(center[0], center[1]), cornerPts, metadata, ftcPose, rawPose, robotPose, captureTimeNanos)); } ApriltagDetectionJNI.freeDetectionList(ptrDetectionArray); @@ -316,6 +376,53 @@ ArrayList runAprilTagDetectorForMultipleTagSizes(long capture return new ArrayList<>(); } + private Pose3D computeRobotPose(AprilTagPoseRaw rawPose, AprilTagMetadata metadata, long acquisitionTime) + { + // Compute transformation matrix of tag pose in field reference frame + float tagInFieldX = metadata.fieldPosition.get(0); + float tagInFieldY = metadata.fieldPosition.get(1); + float tagInFieldZ = metadata.fieldPosition.get(2); + OpenGLMatrix tagInFieldR = new OpenGLMatrix(metadata.fieldOrientation.toMatrix()); + OpenGLMatrix tagInFieldFrame = OpenGLMatrix.identityMatrix() + .translated(tagInFieldX, tagInFieldY, tagInFieldZ) + .multiplied(tagInFieldR); + + // Compute transformation matrix of camera pose in tag reference frame + float tagInCameraX = (float) DistanceUnit.INCH.fromUnit(outputUnitsLength, rawPose.x); + float tagInCameraY = (float) DistanceUnit.INCH.fromUnit(outputUnitsLength, rawPose.y); + float tagInCameraZ = (float) DistanceUnit.INCH.fromUnit(outputUnitsLength, rawPose.z); + OpenGLMatrix tagInCameraR = new OpenGLMatrix((rawPose.R)); + OpenGLMatrix cameraInTagFrame = OpenGLMatrix.identityMatrix() + .translated(tagInCameraX, tagInCameraY, tagInCameraZ) + .multiplied(tagInCameraR) + .inverted(); + + // Compute transformation matrix of robot pose in field frame + OpenGLMatrix robotInFieldFrame = + tagInFieldFrame + .multiplied(cameraInTagFrame) + .multiplied(robotInCameraFrame); + + // Extract robot location + VectorF robotInFieldTranslation = robotInFieldFrame.getTranslation(); + Position robotPosition = new Position(DistanceUnit.INCH, + robotInFieldTranslation.get(0), + robotInFieldTranslation.get(1), + robotInFieldTranslation.get(2), + acquisitionTime).toUnit(outputUnitsLength); + + // Extract robot orientation + Orientation robotInFieldOrientation = Orientation.getOrientation(robotInFieldFrame, + AxesReference.INTRINSIC, AxesOrder.ZXY, outputUnitsAngle); + YawPitchRollAngles robotOrientation = new YawPitchRollAngles(outputUnitsAngle, + robotInFieldOrientation.firstAngle, + robotInFieldOrientation.secondAngle, + robotInFieldOrientation.thirdAngle, + acquisitionTime); + + return new Pose3D(robotPosition, robotOrientation); + } + private final Object drawSync = new Object(); @Override diff --git a/Vision/src/main/java/org/openftc/easyopencv/OpenCvCameraFactory.java b/Vision/src/main/java/org/openftc/easyopencv/OpenCvCameraFactory.java index f417bd0c..896881af 100644 --- a/Vision/src/main/java/org/openftc/easyopencv/OpenCvCameraFactory.java +++ b/Vision/src/main/java/org/openftc/easyopencv/OpenCvCameraFactory.java @@ -10,7 +10,6 @@ public static OpenCvCameraFactory getInstance() { return instance; } - /* * Internal */ diff --git a/Vision/src/main/java/org/openftc/easyopencv/OpenCvPipeline.java b/Vision/src/main/java/org/openftc/easyopencv/OpenCvPipeline.java index 13cac400..2daee177 100644 --- a/Vision/src/main/java/org/openftc/easyopencv/OpenCvPipeline.java +++ b/Vision/src/main/java/org/openftc/easyopencv/OpenCvPipeline.java @@ -1,3 +1,24 @@ +/* + * Copyright (c) 2019 OpenFTC Team + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + package org.openftc.easyopencv; import android.graphics.Canvas; diff --git a/Vision/src/main/java/org/openftc/easyopencv/OpenCvViewRenderer.java b/Vision/src/main/java/org/openftc/easyopencv/OpenCvViewRenderer.java index 39596a3c..ea186167 100644 --- a/Vision/src/main/java/org/openftc/easyopencv/OpenCvViewRenderer.java +++ b/Vision/src/main/java/org/openftc/easyopencv/OpenCvViewRenderer.java @@ -75,7 +75,7 @@ public OpenCvViewRenderer(boolean renderingOffsceen, String fpsMeterDescriptor) metricsScale = 1.0f; fpsMeterTextSize = 26.2f * metricsScale; - statBoxW = (int) (450 * metricsScale); + statBoxW = (int) (430 * metricsScale); statBoxH = (int) (120 * metricsScale); statBoxTextLineSpacing = (int) (35 * metricsScale); statBoxLTxtMargin = (int) (5 * metricsScale); diff --git a/build.gradle b/build.gradle index d99a652a..33730078 100644 --- a/build.gradle +++ b/build.gradle @@ -10,7 +10,7 @@ buildscript { log4j_version = "2.17.1" opencv_version = "4.7.0-0" apriltag_plugin_version = "2.0.0-C" - skiko_version = "0.7.75" + skiko_version = "0.8.15" classgraph_version = "4.8.108" opencsv_version = "5.5.2" @@ -37,7 +37,7 @@ plugins { allprojects { group 'com.github.deltacv' - version '3.7.0' + version '3.7.1' apply plugin: 'java'