/** * Given the interpolated index, compute the angle from the 3 indexes. The angle for each index * is computed from the weighted gradients. * @param offset Interpolated index offset relative to index0. range -1 to 1 * @return Interpolated angle. */ double interpolateAngle(int index0, int index1, int index2, double offset) { double angle1 = Math.atan2(histogramY[index1],histogramX[index1]); double deltaAngle; if( offset < 0 ) { double angle0 = Math.atan2(histogramY[index0],histogramX[index0]); deltaAngle = UtilAngle.dist(angle0,angle1); } else { double angle2 = Math.atan2(histogramY[index2], histogramX[index2]); deltaAngle = UtilAngle.dist(angle2,angle1); } return UtilAngle.bound(angle1 + deltaAngle*offset); }
if( polar.distance < 0 ) { polar.distance = -polar.distance; polar.angle = UtilAngle.bound(polar.angle + Math.PI);
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJointBasics oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clamp(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clipToMinMax(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }