0)
+ mOpMode.telemetry.addData("scanline(det): ",
+ scanline.substring(0,patternStart) +
+ scanline.substring(patternStart,patternStart+patternSize).toUpperCase() +
+ scanline.substring(patternStart+patternSize)
+ );
+
+ // this step is done when the target reaches the desired size --
+ // (i.e. we're at the desired distance from it) depending on whether we're going toward
+ // it (speed > 0) or away from it (speed < 0)
+ boolean bDone = (mPatternSize > 0) && // don't quit just because we "lost signal"
+ ((mPower > 0) ? (patternSize > mPatternSize) : (patternSize < mPatternSize));
+
+ // when the step is done, turn off all motors
+ if (bDone) {
+ for (DcMotor motor : mMotors)
+ motor.setPower(0);
+ }
+
+ // return done-ness
+ return (bDone);
+ }
+
+}
+
+public class CameraDriveTest1Op extends OpMode {
+
+ AutoLib.Sequence mSequence; // the root of the sequence tree
+ boolean bDone; // true when the programmed sequence is done
+
+ CameraLib.CameraAcquireFrames mCamAcqFr;
+ DcMotor mMotors[];
+
+ public CameraDriveTest1Op() {
+ }
+
+ public void init() {
+ // get the camera that our sequence will use
+ mCamAcqFr = new CameraLib.CameraAcquireFrames();
+ if (mCamAcqFr.init(1) == false) // init camera at the smallest available size
+ telemetry.addData("error: ", "cannot initialize camera");
+
+ AutoLib.HardwareFactory mf = null;
+ final boolean debug = true;
+ if (debug)
+ mf = new AutoLib.TestHardwareFactory(this);
+ else
+ mf = new AutoLib.RealHardwareFactory(this);
+
+ // get the motors: depending on the factory we created above, these may be
+ // either dummy motors that just log data or real ones that drive the hardware
+ // assumed order is fr, br, fl, bl
+ mMotors = new DcMotor[4];
+ mMotors[0] = mf.getDcMotor("fr");
+ mMotors[1] = mf.getDcMotor("br");
+ mMotors[2] = mf.getDcMotor("fl");
+ mMotors[3] = mf.getDcMotor("bl");
+ mMotors[2].setDirection(DcMotor.Direction.REVERSE);
+ mMotors[3].setDirection(DcMotor.Direction.REVERSE);
+
+ // create an autonomous sequence with two steps in it (for now)
+ // create the root Sequence for this autonomous OpMode
+ mSequence = new AutoLib.LinearSequence();
+
+ // define the color pattern we're looking for:
+ // String pattern = "r+[w0-7]+([cb]+)[^cb]*"; // RED, WHITE, CYAN/BLUE shelter lights pattern
+ String pattern = "r+([gc]+)b+"; // RGB stripes (where green may read as cyan)
+
+ // add a camera-directed Step to the root Sequence that advances until the pattern is >5 bands of 6 pixels
+ mSequence.add(new CameraDriveStep1(this, mCamAcqFr, pattern, mMotors, 0.5f, 5, 6));
+
+ // add a second camera-directed Step that backs away until the pattern is <2 bands of 6 pixels
+ mSequence.add(new CameraDriveStep1(this, mCamAcqFr, pattern, mMotors, -0.5f, 2, 6));
+
+ // start out not-done
+ bDone = false;
+ }
+
+ public void loop() {
+ // until we're done, keep looping through the current Step(s)
+ if (!bDone)
+ bDone = mSequence.loop(); // returns true when we're done
+ else
+ telemetry.addData("sequence finished", "");
+ }
+
+ public void stop() {
+ mCamAcqFr.stop();
+ telemetry.addData("stop() called", "");
+ }
+}
+
+*/
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/CameraTestOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/CameraTestOp.java
new file mode 100644
index 00000000000..3ed11d08a14
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/CameraTestOp.java
@@ -0,0 +1,127 @@
+/**
+ * CameraTestOp - a simple operating mode that tries to acquire an image from the
+ * phone camera and get some data from the image
+ * Created by phanau on 12/9/15.
+ */
+
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import android.hardware.Camera;
+
+import org.firstinspires.ftc.teamcode.libraries.CameraLib;
+
+
+@Autonomous(name="Test: CameraLib Test 1", group ="Test")
+@Disabled
+public class CameraTestOp extends OpMode {
+
+ int mLoopCount;
+ CameraLib.CameraAcquireFrames mCamAcqFr;
+
+
+ // Constructor
+ public CameraTestOp() {
+ mCamAcqFr = new CameraLib.CameraAcquireFrames();
+ }
+
+ @Override
+ public void init() {
+ mLoopCount = 0;
+
+ if (mCamAcqFr.init(2) == false) // init camera at 2nd smallest size
+ telemetry.addData("error: ", "cannot initialize camera");
+
+ }
+
+ public void loop() {
+ // post some debug data
+ telemetry.addData("loop count:", mLoopCount++);
+ telemetry.addData("version: ", "1.3");
+
+ int[] topScan;
+ int[] middleScan;
+ int[] bottomScan;
+
+ // get most recent frame from camera (may be same as last time or null)
+ CameraLib.CameraImage frame = mCamAcqFr.loop();
+
+ // log debug info ...
+ if (frame != null) {
+
+ // process the current frame
+ // ... "move toward the light..."
+
+ // log data about the most current image to driver station every loop so it stays up long enough to read
+ Camera.Size camSize = frame.cameraSize();
+ telemetry.addData("preview camera size: ", String.valueOf(camSize.width) + "x" + String.valueOf(camSize.height));
+ telemetry.addData("preview data size:", frame.dataSize());
+ telemetry.addData("preview rgb(center):", String.format("%08X", frame.getPixel(camSize.width / 2, camSize.height / 2)));
+ telemetry.addData("frame number: ", mCamAcqFr.frameCount());
+
+ // log text representations of several significant scanlines
+
+ topScan = frame.scanlineValue(camSize.height / 3);
+ middleScan = frame.scanlineValue(camSize.height / 2);
+ bottomScan = frame.scanlineValue(camSize.height * 2 / 3);
+
+ //telemetry.addData("value a(1/3): ", topScan);
+ //telemetry.addData("value b(1/2): ", middleScan);
+ //telemetry.addData("value c(2/3): ", bottomScan);
+
+ final int bandSize = 10;
+ telemetry.addData("hue a(1/3): ", frame.scanlineHue(camSize.height / 3, bandSize));
+ telemetry.addData("hue b(1/2): ", frame.scanlineHue(camSize.height / 2, bandSize));
+ telemetry.addData("hue c(2/3): ", frame.scanlineHue(2*camSize.height / 3, bandSize));
+
+ int topThresh = threshFind(topScan);
+ int middleThresh = threshFind(middleScan);
+ int bottomThresh = threshFind(bottomScan);
+
+ telemetry.addData("Top Thresh", topThresh);
+ telemetry.addData("Middle Thresh", middleThresh);
+ telemetry.addData("Bottom Thresh", bottomThresh);
+
+ int topPos = findAvgOverThresh(topScan, topThresh);
+ int middlePos = findAvgOverThresh(middleScan, middleThresh);
+ int bottomPos = findAvgOverThresh(bottomScan, bottomThresh);
+
+ telemetry.addData("Top Pos", topPos);
+ telemetry.addData("Middle Pos", middlePos);
+ telemetry.addData("Bottom Pos", bottomPos);
+ }
+ }
+
+ public void stop() {
+ mCamAcqFr.stop();
+ }
+
+ //takes a n array of bytes, and returns (min + max)/2 for a threshold
+ private int threshFind(int[] ray){
+ int min = 0;
+ int max = 0;
+ for(int i = 0; i < ray.length; i++){
+ if(min > ray[i]) min = ray[i];
+ else if(max < ray[i]) max = ray[i];
+ }
+
+ return (min + max) / 2;
+ }
+
+ private int findAvgOverThresh(int[] ray, int thresh){
+ int totalPos = 0;
+ int totalNum = 0;
+ for(int i = 0; i < ray.length; i++){
+ if(ray[i] >= thresh){
+ totalPos += i;
+ totalNum++;
+ }
+ }
+
+ return totalPos/totalNum;
+ }
+
+
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContServoCalibrate.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContServoCalibrate.java
new file mode 100644
index 00000000000..d6d2b0273cf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContServoCalibrate.java
@@ -0,0 +1,90 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 3/6/2018.
+ */
+/*
+@TeleOp(name = "Intake Servo Calibrate", group = "Main")
+@Disabled
+public class ContServoCalibrate extends OpMode {
+
+ private enum Incs {
+ SMALL(0.01),
+ MEDIUM(0.05),
+ EHH(0.1),
+ HOLYMOTHER(0.5);
+
+ public double inc;
+ Incs(double inc) {
+ this.inc = inc;
+ }
+ }
+
+ BotHardware bot = new BotHardware(this);
+ boolean lastDPad = false;
+ boolean lastA = false;
+ boolean front = false;
+ int incIndex = 0;
+
+ double frontPower = 0;
+ double backPower = 0;
+
+ @Override
+ public void init() {
+
+ telemetry.addData("Status", "Initialized");
+
+ //currentServo = robot.leftServo;
+
+ // hardware maps
+ bot.init();
+ }
+
+ @Override
+ public void start() {}
+
+ @Override
+ public void loop() {
+ if(!lastDPad) {
+ if (gamepad1.dpad_up) {
+ if(front) frontPower = Range.clip(frontPower + Incs.values()[incIndex].inc, -1, 1);
+ else backPower = Range.clip(backPower + Incs.values()[incIndex].inc, -1, 1);
+ }
+ else if (gamepad1.dpad_down) {
+ if(front) frontPower = Range.clip(frontPower - Incs.values()[incIndex].inc, -1, 1);
+ else backPower = Range.clip(backPower - Incs.values()[incIndex].inc, -1, 1);
+ }
+ else if (gamepad1.dpad_left) incIndex = Range.clip(incIndex - 1, 0, Incs.values().length - 1);
+ else if (gamepad1.dpad_right) incIndex = Range.clip(incIndex + 1, 0, Incs.values().length - 1);
+
+ if(gamepad1.dpad_up || gamepad1.dpad_down) {
+ if(front) {
+ BotHardware.ContiniuosServoE.FrontSuckLeft.servo.setPower(frontPower);
+ BotHardware.ContiniuosServoE.FrontSuckRight.servo.setPower(frontPower);
+ }
+ else {
+ BotHardware.ContiniuosServoE.SuckLeft.servo.setPower(backPower);
+ BotHardware.ContiniuosServoE.SuckRight.servo.setPower(backPower);
+ }
+ }
+ }
+
+ lastDPad = gamepad1.dpad_up || gamepad1.dpad_right || gamepad1.dpad_left || gamepad1.dpad_down;
+
+ if(!lastA && gamepad1.a) front = !front;
+ lastA = gamepad1.a;
+
+ telemetry.addData("Intake", front ? "Front" : "Back");
+ telemetry.addData("Servo Front", frontPower);
+ telemetry.addData("Servo Back", backPower);
+ telemetry.addData("Increment", Incs.values()[incIndex].inc);
+ }
+}
+*/
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContiniousServoAdjust.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContiniousServoAdjust.java
new file mode 100644
index 00000000000..04ce5f769d5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContiniousServoAdjust.java
@@ -0,0 +1,65 @@
+/*
+ Main robot teleop program
+ */
+
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/*
+@TeleOp(name = "Cont Servo Calibrate", group = "Main")
+@Disabled
+public class ContiniousServoAdjust extends OpMode {
+
+ private enum Incs {
+ SMALL(0.01),
+ MEDIUM(0.05),
+ EHH(0.1),
+ HOLYMOTHER(0.5);
+
+ public double inc;
+ Incs(double inc) {
+ this.inc = inc;
+ }
+ }
+
+ BotHardware bot = new BotHardware(this);
+ boolean lastDPad = false;
+ int incIndex = 0;
+
+ @Override
+ public void init() {
+
+ telemetry.addData("Status", "Initialized");
+
+ //currentServo = robot.leftServo;
+
+ // hardware maps
+ bot.init();
+ }
+
+ @Override
+ public void start() {}
+
+ @Override
+ public void loop() {
+ telemetry.addData("Servo", BotHardware.ContiniuosServoE.FrontSuckLeft.servo.getPower());
+ telemetry.addData("Increment", Incs.values()[incIndex].inc);
+ if(!lastDPad) {
+ if (gamepad1.dpad_up)
+ BotHardware.ContiniuosServoE.FrontSuckLeft.servo.setPower(Range.clip(BotHardware.ContiniuosServoE.FrontSuckLeft.servo.getPower() + Incs.values()[incIndex].inc, -1, 1));
+ else if (gamepad1.dpad_down)
+ BotHardware.ContiniuosServoE.FrontSuckLeft.servo.setPower(Range.clip(BotHardware.ContiniuosServoE.FrontSuckLeft.servo.getPower() - Incs.values()[incIndex].inc, -1, 1));
+ else if (gamepad1.dpad_left) incIndex = Range.clip(incIndex - 1, 0, Incs.values().length - 1);
+ else if (gamepad1.dpad_right) incIndex = Range.clip(incIndex + 1, 0, Incs.values().length - 1);
+ }
+
+ lastDPad = gamepad1.dpad_up || gamepad1.dpad_right || gamepad1.dpad_left || gamepad1.dpad_down;
+ }
+}
+*/
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContiniuosMess.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContiniuosMess.java
new file mode 100644
index 00000000000..bad8fc80f23
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ContiniuosMess.java
@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.AnalogOutput;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.PwmControl;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.ServoControllerEx;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 3/6/2018.
+ */
+
+@TeleOp(name="Servo Fix")
+//@Disabled
+public class ContiniuosMess extends OpMode {
+ private enum Incs {
+ SMALL(10),
+ MEDIUM(25),
+ EHH(100),
+ HOLYMOTHER(500);
+
+ public double inc;
+ Incs(double inc) {
+ this.inc = inc;
+ }
+ }
+
+
+ private CRServo stupid;
+ private ServoControllerEx stupidControl;
+ private DigitalChannel out;
+ private int port;
+ private boolean lastDPad = false;
+ private boolean lastA;
+ private int incIndex = 0;
+
+ public void init() {
+ stupid = hardwareMap.get(CRServo.class, "crl");
+ stupidControl = (ServoControllerEx) stupid.getController();
+ out = hardwareMap.get(DigitalChannel.class, "stupid");
+ out.setMode(DigitalChannel.Mode.OUTPUT);
+ stupid.getPortNumber();
+ }
+
+ public void start() {
+ stupid.setPower(1);
+ }
+
+ public void loop() {
+ PwmControl.PwmRange range = stupidControl.getServoPwmRange(port);
+ if(!lastDPad) {
+ if (gamepad1.dpad_up) range = new PwmControl.PwmRange(100, range.usPulseUpper + Incs.values()[incIndex].inc);
+ else if (gamepad1.dpad_down) range = new PwmControl.PwmRange(100, range.usPulseUpper - Incs.values()[incIndex].inc);
+ else if (gamepad1.dpad_left) incIndex = Range.clip(incIndex - 1, 0, Incs.values().length - 1);
+ else if (gamepad1.dpad_right) incIndex = Range.clip(incIndex + 1, 0, Incs.values().length - 1);
+ stupidControl.setServoPwmRange(port, range);
+ stupid.setPower(1);
+ }
+
+ lastDPad = gamepad1.dpad_up || gamepad1.dpad_right || gamepad1.dpad_left || gamepad1.dpad_down;
+
+ if(!lastA && gamepad1.a) out.setState(!out.getState());
+ lastA = gamepad1.a;
+
+ telemetry.addData("Servo Duty Cycle", range.usPulseUpper);
+ telemetry.addData("Increment", Incs.values()[incIndex].inc);
+ telemetry.addData("Digital Out", out.getState());
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/GyroTurnDemo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/GyroTurnDemo.java
new file mode 100644
index 00000000000..c70e9a536db
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/GyroTurnDemo.java
@@ -0,0 +1,63 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.HeadingSensor;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 12/1/2017.
+ */
+
+@Autonomous(name="Turn Testing", group="test")
+@Disabled
+public class GyroTurnDemo extends OpMode {
+ BotHardware bot = new BotHardware(this);
+
+ AutoLib.Sequence mSeq = new AutoLib.LinearSequence();
+
+ //parameters for gyro PID, but cranked up
+ float Kp5 = 3.0f; // degree heading proportional term correction per degree of deviation
+ float Ki5 = 0.0f; // ... integrator term
+ float Kd5 = 0; // ... derivative term
+ float Ki5Cutoff = 0.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID motorPID = new SensorLib.PID(Kp5, Ki5, Kd5, Ki5Cutoff);
+
+ public void init() {
+ bot.init();
+
+ mSeq.add(new AutoLib.GyroTurnStep(this, 90, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 45.0f, 360.0f, motorPID, 1.0f, 3, true));
+
+ //mSeq.add(new AutoLib.AzimuthTimedDriveStep(this, 90, bot.getHeadingSensor(), motorPID, bot.getMotorVelocityShimArray(), 270.0f, 2.0f, true, -450.0f, 450.0f));
+
+ }
+
+ public void start() {
+
+ }
+
+ public void loop() {
+ if(mSeq.loop()) requestOpModeStop();
+ }
+
+ private static class GyroStopStep extends AutoLib.Step {
+ private final HeadingSensor gyro;
+ private final float heading;
+ private final int error;
+ GyroStopStep(HeadingSensor gyro, float heading, int error) {
+ this.gyro = gyro;
+ this.heading = heading;
+ this.error = error;
+ }
+
+ public boolean loop() {
+ return Math.abs(gyro.getHeading() - heading) <= error;
+ }
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/OpenCVDebug.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/OpenCVDebug.java
new file mode 100644
index 00000000000..fa1b892f177
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/OpenCVDebug.java
@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.LineFollowLib;
+import org.firstinspires.ftc.teamcode.libraries.OpenCVLib;
+import org.opencv.core.Mat;
+
+/**
+ * Created by Robotics on 10/28/2016.
+ */
+
+@Autonomous(name = "OpenCV Debug", group = "Line Follow")
+@Disabled
+public class OpenCVDebug extends OpenCVLib {
+
+ private int[] yValStore = new int[3];
+
+ @Override
+ public void init(){
+ initOpenCV();
+ startCamera();
+
+ Mat frame = getCameraFrame();
+
+ //init scanline Y values
+ yValStore[0] = frame.rows() / 4;
+ yValStore[1] = frame.rows() / 2;
+ yValStore[2] = (frame.rows() * 3) / 4;
+ }
+
+ @Override
+ public void start(){
+
+ }
+
+ @Override
+ public void loop(){
+ Mat frame = getCameraFrame();
+ telemetry.addData("Find Avg", LineFollowLib.scanlineAvgDebug(frame, yValStore[1], this));
+ }
+
+ @Override
+ public void stop(){
+ stopCamera();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/SensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/SensorTest.java
new file mode 100644
index 00000000000..36f940536da
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/SensorTest.java
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+
+/**
+ * simple example of using a Step that makes a bot with "squirrely wheels" drive along a given course
+ * Created by phanau on 10/31/16.
+ */
+
+
+// simple example sequence that tests either of gyro-based AzimuthCountedDriveStep or AzimuthTimedDriveStep to drive along a square path
+@Autonomous(name="Testing Sensor Code", group="Test")
+@Disabled
+public class SensorTest extends OpMode {
+
+ BotHardwareOld bot;
+
+ @Override
+ public void init(){
+ bot = new BotHardwareOld();
+
+ bot.init(this, false);
+
+ }
+
+ @Override
+ public void start(){
+ //Vuf.start();
+ }
+
+ @Override
+ public void loop() {
+ telemetry.addData("Color Sensor Left Red", bot.leftSensor.red());
+ telemetry.addData("Color Sensor Left Blue", bot.leftSensor.blue());
+ telemetry.addData("Color Sensor Right Red", bot.rightSensor.red());
+ telemetry.addData("Color Sensor Right Blue", bot.rightSensor.blue());
+ telemetry.addData("Ultra Left", bot.distSensorLeft.getUltrasonicLevel());
+ telemetry.addData("Ultra Right", bot.distSensorRight.getUltrasonicLevel());
+ }
+
+ @Override
+ public void stop() {
+ super.stop();
+ //Vuf.stop();
+ }
+}
+
+
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ServoAdjust.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ServoAdjust.java
new file mode 100644
index 00000000000..6cc02b3d0ae
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ServoAdjust.java
@@ -0,0 +1,62 @@
+/*
+ Main robot teleop program
+ */
+
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+@TeleOp(name = "Servo Calibrate", group = "Main")
+//@Disabled
+public class ServoAdjust extends OpMode {
+
+ private enum Incs {
+ SMALL(0.01),
+ MEDIUM(0.05),
+ EHH(0.1),
+ HOLYMOTHER(0.5);
+
+ public double inc;
+ Incs(double inc) {
+ this.inc = inc;
+ }
+ }
+
+ BotHardware bot = new BotHardware(this);
+ boolean lastDPad = false;
+ int incIndex = 0;
+
+ @Override
+ public void init() {
+
+ telemetry.addData("Status", "Initialized");
+
+ //currentServo = robot.leftServo;
+
+ // hardware maps
+ bot.init();
+ }
+
+ @Override
+ public void start() {}
+
+ @Override
+ public void loop() {
+ telemetry.addData("Servo", bot.getStick().getPosition());
+ telemetry.addData("Increment", Incs.values()[incIndex].inc);
+ if(!lastDPad) {
+ if (gamepad1.dpad_up)
+ bot.getStick().setPosition(Range.clip(bot.getStick().getPosition() + Incs.values()[incIndex].inc, -1, 1));
+ else if (gamepad1.dpad_down)
+ bot.getStick().setPosition(Range.clip(bot.getStick().getPosition() - Incs.values()[incIndex].inc, -1, 1));
+ else if (gamepad1.dpad_left) incIndex = Range.clip(incIndex - 1, 0, Incs.values().length - 1);
+ else if (gamepad1.dpad_right) incIndex = Range.clip(incIndex + 1, 0, Incs.values().length - 1);
+ }
+
+ lastDPad = gamepad1.dpad_up || gamepad1.dpad_right || gamepad1.dpad_left || gamepad1.dpad_down;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ServoDegreeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ServoDegreeTest.java
new file mode 100644
index 00000000000..e67b54d47dc
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/ServoDegreeTest.java
@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+import org.firstinspires.ftc.teamcode.libraries.hardware.WhackyDistLib;
+
+/**
+ * Created by Noah on 12/28/2017.
+ */
+
+@TeleOp(name = "Stick Degrees Test", group = "Main")
+@Disabled
+public class ServoDegreeTest extends OpMode {
+
+ private enum Incs {
+ SMALL(1),
+ MEDIUM(5),
+ EHH(10),
+ HOLYMOTHER(50);
+
+ public double inc;
+ Incs(double inc) {
+ this.inc = inc;
+ }
+ }
+
+ BotHardware bot = new BotHardware(this);
+ boolean lastDPad = false;
+ int incIndex = 0;
+
+ double currentDegrees = 90;
+
+ @Override
+ public void init() {
+
+ telemetry.addData("Status", "Initialized");
+
+ //currentServo = robot.leftServo;
+
+ // hardware maps
+ bot.init();
+ }
+
+ @Override
+ public void start() {}
+
+ @Override
+ public void loop() {
+ telemetry.addData("Servo", bot.getStick().getPosition());
+ telemetry.addData("Degrees", currentDegrees);
+ telemetry.addData("Distance", WhackyDistLib.getWhackyDistance(currentDegrees, DistanceUnit.INCH));
+ telemetry.addData("Increment", Incs.values()[incIndex].inc);
+ if(!lastDPad) {
+ if (gamepad1.dpad_up)
+ bot.getStick().setPosition(Range.clip(WhackyDistLib.getWhackyPosFromDegrees(currentDegrees += Incs.values()[incIndex].inc), -1, 1));
+ else if (gamepad1.dpad_down)
+ bot.getStick().setPosition(Range.clip(WhackyDistLib.getWhackyPosFromDegrees(currentDegrees -= Incs.values()[incIndex].inc), -1, 1));
+ else if (gamepad1.dpad_left) incIndex = Range.clip(incIndex - 1, 0, Incs.values().length - 1);
+ else if (gamepad1.dpad_right) incIndex = Range.clip(incIndex + 1, 0, Incs.values().length - 1);
+ }
+
+ lastDPad = gamepad1.dpad_up || gamepad1.dpad_right || gamepad1.dpad_left || gamepad1.dpad_down;
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/UltraHoneDebug.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/UltraHoneDebug.java
new file mode 100644
index 00000000000..8c9102def97
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/UltraHoneDebug.java
@@ -0,0 +1,155 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.GyroCorrectStep;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+import org.firstinspires.ftc.teamcode.libraries.hardware.MatbotixUltra;
+
+/**
+ * Created by Noah on 2/14/2018.
+ */
+
+@Autonomous(name="Ultra Calibrate")
+@Disabled
+public class UltraHoneDebug extends OpMode {
+ private static final int BACKUP_COUNTS = 980;
+
+ AutoLib.Sequence mSeq = new AutoLib.LinearSequence();
+ BotHardware bot = new BotHardware(this);
+ protected MatbotixUltra frontUltra;
+ protected MatbotixUltra backUltra;
+
+ private int counts = 32;
+ private boolean lastDPad = false;
+ private boolean done = false;
+
+ private int startCounts = 0;
+ private int endCounts = 0;
+
+ //parameters for gyro turning
+ float Kp5 = 15.0f; // degree heading proportional term correction per degree of deviation
+ float Ki5 = 0.0f; // ... integrator term
+ float Kd5 = 0; // ... derivative term
+ float Ki5Cutoff = 0.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID motorPID = new SensorLib.PID(Kp5, Ki5, Kd5, Ki5Cutoff);
+
+ public void init() {
+ frontUltra = new MatbotixUltra(hardwareMap.get(I2cDeviceSynch.class, "ultrafront"), 100);
+ backUltra = new MatbotixUltra(hardwareMap.get(I2cDeviceSynch.class, "ultraback"), 100);
+ frontUltra.initDevice();
+ backUltra.initDevice();
+ frontUltra.startDevice();
+ backUltra.startDevice();
+ bot.init();
+ }
+
+ public void init_loop() {
+ if(!lastDPad) {
+ if(gamepad1.dpad_up) counts++;
+ if(gamepad1.dpad_down) counts--;
+ }
+ lastDPad = gamepad1.dpad_up || gamepad1.dpad_down;
+ }
+
+ public void start() {
+ //initialize sequence
+ mSeq.add(new UltraHoneStep(this, frontUltra, counts, 0, 5,
+ new SensorLib.PID(15f, 0.15f, 0, 10),
+ new GyroCorrectStep(this, 0, bot.getHeadingSensor(), new SensorLib.PID(-16, 0, 0, 0), bot.getMotorVelocityShimArray(), 0.0f, 55.0f, 400.0f)));
+ mSeq.add(new AutoLib.GyroTurnStep(this, 90, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 55.0f, 650.0f, motorPID, 0.5f, 10, true));
+ mSeq.add(new AutoLib.TimedServoStep(BotHardware.ServoE.stickBase.servo, BotHardware.ServoE.stickBaseCenter, 0.25, false));
+ mSeq.add(new AutoLib.TimedServoStep(BotHardware.ServoE.stick.servo, 0.85, 0.25, false));
+ }
+
+ public void loop() {
+ if(!done) done = mSeq.loop();
+ telemetry.addData("Start Counts", startCounts);
+ telemetry.addData("End Counts", endCounts);
+ telemetry.addData("Total Distance", startCounts - endCounts);
+ }
+
+ private class UltraHoneStep extends AutoLib.Step {
+ private final OpMode mode;
+ private final MatbotixUltra ultra;
+ private final int dist;
+ private final int error;
+ private final int count;
+ private final SensorLib.PID errorPid;
+ private final GyroCorrectStep gyroStep;
+
+ private double lastTime = 0;
+ private int currentCount = 0;
+
+ UltraHoneStep(OpMode mode, MatbotixUltra ultra, int dist, int error, int count, SensorLib.PID errorPid, GyroCorrectStep gyroStep) {
+ this.mode = mode;
+ this.ultra = ultra;
+ this.dist = dist;
+ this.error = error;
+ this.count = count;
+ this.errorPid = errorPid;
+ this.gyroStep = gyroStep;
+ }
+
+ public boolean loop() {
+ super.loop();
+ if(firstLoopCall()) {
+ lastTime = mode.getRuntime() - 1;
+ startCounts = BotHardware.Motor.frontLeft.motor.getCurrentPosition();
+ }
+ //get the distance and error
+ final int read = ultra.getReading();
+ final int curError = dist - read;
+ //if we found it, stop
+ //if the peak is within stopping margin, stop
+ if(Math.abs(curError) <= error) {
+ setMotorsWithoutGyro(0);
+ if (++currentCount >= count) {
+ endCounts = BotHardware.Motor.frontLeft.motor.getCurrentPosition();
+ return true;
+ }
+ else return false;
+ }
+ else currentCount = 0;
+ //PID
+ final double time = mode.getRuntime();
+ final float pError = errorPid.loop(curError, (float)(time - lastTime));
+ lastTime = time;
+ mode.telemetry.addData("power error", pError);
+ //cut out a middle range, but handle positive and negative
+ final float power;
+ if(pError >= 0) power = Range.clip(pError, gyroStep.getMinPower(), gyroStep.getMaxPower());
+ else power = Range.clip(pError, -gyroStep.getMaxPower(), -gyroStep.getMinPower());
+ /*
+ if(gyroStep.getStartPower() >= 0){
+ if(pError >= 0) power = Range.clip(gyroStep.getStartPower() + pError, gyroStep.getMinPower(), gyroStep.getMaxPower());
+ else power = Range.clip(pError - gyroStep.getStartPower(), -gyroStep.getMaxPower(), -gyroStep.getMinPower());
+ }
+ else {
+ if(pError >= 0) power = Range.clip(gyroStep.getStartPower() - pError, -gyroStep.getMaxPower(), -gyroStep.getMinPower());
+ else power = Range.clip(Math.abs(gyroStep.getStartPower() + pError), gyroStep.getMinPower(), gyroStep.getMaxPower());
+ }
+ */
+ gyroStep.setPower(power);
+ gyroStep.loop();
+ //telem
+ mode.telemetry.addData("Power", -power);
+ mode.telemetry.addData("Ultra error", curError);
+ mode.telemetry.addData("Ultra", read);
+ //return
+ return false;
+ }
+
+ private void setMotorsWithoutGyro(float power) {
+ for(DcMotor motor : gyroStep.getMotors()) motor.setPower(power);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/VelocityLatency.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/VelocityLatency.java
new file mode 100644
index 00000000000..2de8c88910f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/diagnostic/VelocityLatency.java
@@ -0,0 +1,65 @@
+package org.firstinspires.ftc.teamcode.opmodes.diagnostic;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.lynx.commands.core.LynxGetBulkInputDataCommand;
+import com.qualcomm.hardware.lynx.commands.core.LynxGetBulkInputDataResponse;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+/**
+ * Created by Noah on 5/9/2018.
+ */
+
+@TeleOp(name = "Velocity Latency")
+public class VelocityLatency extends OpMode {
+ private LynxModule module;
+ private long lastMs;
+ private DcMotorEx backMotor;
+ private DcMotorEx frontMotor;
+
+
+ public void init() {
+ module = hardwareMap.get(LynxModule.class, "m");
+ backMotor = hardwareMap.get(DcMotorEx.class, "bl");
+ frontMotor = hardwareMap.get(DcMotorEx.class, "fl");
+ lastMs = System.currentTimeMillis();
+ }
+
+ public void init_loop() {
+ long time = System.currentTimeMillis();
+
+ telemetry.addData("Position 0", backMotor.getCurrentPosition());
+ telemetry.addData("Position 1", frontMotor.getCurrentPosition());
+ telemetry.addData("Velocity 0", backMotor.getVelocity(AngleUnit.RADIANS));
+ telemetry.addData("Velocity 1", frontMotor.getVelocity(AngleUnit.RADIANS));
+ telemetry.addData("Latency", time - lastMs);
+ lastMs = time;
+ }
+
+ public void start() {
+
+ }
+
+ public void loop() {
+ try {
+ LynxGetBulkInputDataCommand command = new LynxGetBulkInputDataCommand(module);
+ LynxGetBulkInputDataResponse resp = command.sendReceive();
+ long time = System.currentTimeMillis();
+
+ telemetry.addData("Position 0", resp.getEncoder(0));
+ telemetry.addData("Position 1", resp.getEncoder(1));
+ telemetry.addData("Velocity 0", resp.getVelocity(0));
+ telemetry.addData("Velocity 1", resp.getVelocity(1));
+ telemetry.addData("Latency", time - lastMs);
+ lastMs = time;
+ }
+ catch (Exception e) { /* hmmmm */ }
+ }
+
+ public void stop() {
+
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXCollisionDetectionOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXCollisionDetectionOp.java
new file mode 100644
index 00000000000..8d9665d1545
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXCollisionDetectionOp.java
@@ -0,0 +1,205 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.kauailabs.navx.ftc.IDataArrivalSubscriber;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/**
+ * navX-Micro Collision Detection Op
+ *
+ * This is a demo program showing the use of the navX-Micro to implement
+ * a collision detection feature, which can be used to detect events
+ * while driving a robot, such as bumping into a wall or being hit
+ * by another robot.
+ *
+ * The basic principle used within the Collision Detection example
+ * is the calculation of Jerk (which is defined as the change in
+ * acceleration). In the sample code shown below, both the X axis and
+ * the Y axis jerk are calculated, and if either exceeds a threshold,
+ * then a collision has occurred.
+ *
+ * The 'collision threshold' used in these samples will likely need to
+ * be tuned for your robot, since the amount of jerk which constitutes
+ * a collision will be dependent upon the robot mass and expected
+ * maximum velocity of either the robot, or any object which may strike
+ * the robot.
+ *
+ * Note that this example uses the "callback" mechanism to be informed
+ * precisely when new data is received from the navX-Micro.
+ */
+@TeleOp(name = "Concept: navX Collision Detection", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXCollisionDetectionOp extends OpMode implements IDataArrivalSubscriber {
+
+ /* This is the port on the Core Device Interace Module */
+ /* in which the navX-Micro is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+
+ /* Tune this threshold to adjust the sensitivy of the */
+ /* Collision detection. */
+ private final double COLLISION_THRESHOLD_DELTA_G = 0.5;
+
+ double last_world_linear_accel_x;
+ double last_world_linear_accel_y;
+ private ElapsedTime runtime = new ElapsedTime();
+ private AHRS navx_device;
+ private boolean collision_state;
+
+ private final String COLLISION = "Collision";
+ private final String NO_COLLISION = "--------";
+
+ private long last_system_timestamp = 0;
+ private long last_sensor_timestamp = 0;
+
+ private long sensor_timestamp_delta = 0;
+ private long system_timestamp_delta = 0;
+
+ @Override
+ public void init() {
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData);
+ last_world_linear_accel_x = 0.0;
+ last_world_linear_accel_y = 0.0;
+ setCollisionState(false);
+ }
+
+@Override
+ public void start() {
+ navx_device.registerCallback(this);
+}
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+ /*
+ * Code to run when the op mode is first enabled goes here
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init_loop() {
+ telemetry.addData("navX Op Init Loop", runtime.toString());
+ }
+
+ /*
+ * This method will be called repeatedly in a loop
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ */
+ @Override
+ public void loop() {
+
+ boolean connected = navx_device.isConnected();
+ telemetry.addData("1 navX-Device", connected ?
+ "Connected" : "Disconnected" );
+ String gyrocal, motion;
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ if ( connected ) {
+ gyrocal = (navx_device.isCalibrating() ?
+ "CALIBRATING" : "Calibration Complete");
+ motion = (navx_device.isMoving() ? "Moving" : "Not Moving");
+ if ( navx_device.isRotating() ) {
+ motion += ", Rotating";
+ }
+ } else {
+ gyrocal =
+ motion = "-------";
+ }
+ telemetry.addData("2 GyroAccel", gyrocal );
+ telemetry.addData("3 Motion", motion);
+ telemetry.addData("4 Collision", getCollisionString());
+ telemetry.addData("5 Timing", Long.toString(sensor_timestamp_delta) + ", " +
+ Long.toString(system_timestamp_delta) );
+ telemetry.addData("6 Events", Double.toString(navx_device.getUpdateCount()));
+ }
+
+ private String getCollisionString() {
+ return (this.collision_state ? COLLISION : NO_COLLISION);
+ }
+
+ private void setCollisionState( boolean newValue ) {
+ this.collision_state = newValue;
+ }
+
+ /* This callback is invoked by the AHRS class whenever new data is
+ received from the sensor. Note that this callback is occurring
+ within the context of the AHRS class IO thread, and it may
+ interrupt the thread running this opMode. Therefore, it is
+ very important to use thread synchronization techniques when
+ communicating between this callback and the rest of the
+ code in this opMode.
+
+ The difference between the current linear acceleration data in
+ the X and Y axes and that in the last sample is compared. If
+ the absolute value of that difference is greater than the
+ "Collision Detection Threshold", a collision event is declared.
+ */
+
+ @Override
+ public void timestampedDataReceived(long curr_system_timestamp, long curr_sensor_timestamp, Object o) {
+ boolean collisionDetected = false;
+
+ sensor_timestamp_delta = curr_sensor_timestamp - last_sensor_timestamp;
+ system_timestamp_delta = curr_system_timestamp - last_system_timestamp;
+ double curr_world_linear_accel_x = navx_device.getWorldLinearAccelX();
+ double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
+ last_world_linear_accel_x = curr_world_linear_accel_x;
+ double curr_world_linear_accel_y = navx_device.getWorldLinearAccelY();
+ double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
+ last_world_linear_accel_y = curr_world_linear_accel_y;
+
+ if ( ( Math.abs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
+ ( Math.abs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
+ collisionDetected = true;
+ }
+
+ setCollisionState( collisionDetected );
+ }
+
+ @Override
+ public void untimestampedDataReceived(long l, Object o) {
+
+ }
+
+ @Override
+ public void yawReset() {
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXDriveStraightPIDLinearOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXDriveStraightPIDLinearOp.java
new file mode 100644
index 00000000000..fd7031273cb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXDriveStraightPIDLinearOp.java
@@ -0,0 +1,177 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import android.util.Log;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.kauailabs.navx.ftc.navXPIDController;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/*
+ * An example linear op mode where the robot will drive in
+ * a straight line (where the driving direction is guided by
+ * the Yaw angle from a navX-Model device).
+ *
+ * This example uses a simple PID controller configuration
+ * with a P coefficient, and will likely need tuning in order
+ * to achieve optimal performance.
+ *
+ * Note that for the best accuracy, a reasonably high update rate
+ * for the navX-Model sensor should be used. This example uses
+ * the default update rate (50Hz), which may be lowered in order
+ * to reduce the frequency of the updates to the drive system.
+ */
+
+@TeleOp(name = "Concept: navX Drive Straight PID - Linear", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXDriveStraightPIDLinearOp extends LinearOpMode {
+ DcMotor leftMotor;
+ DcMotor rightMotor;
+
+ /* This is the port on the Core Device Interface Module */
+ /* in which the navX-Model Device is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+ private AHRS navx_device;
+ private navXPIDController yawPIDController;
+ private ElapsedTime runtime = new ElapsedTime();
+
+ private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50;
+
+ private final double TARGET_ANGLE_DEGREES = 0.0;
+ private final double TOLERANCE_DEGREES = 2.0;
+ private final double MIN_MOTOR_OUTPUT_VALUE = -1.0;
+ private final double MAX_MOTOR_OUTPUT_VALUE = 1.0;
+ private final double YAW_PID_P = 0.005;
+ private final double YAW_PID_I = 0.0;
+ private final double YAW_PID_D = 0.0;
+
+ private boolean calibration_complete = false;
+
+ public double limit(double a) {
+ return Math.min(Math.max(a, MIN_MOTOR_OUTPUT_VALUE), MAX_MOTOR_OUTPUT_VALUE);
+ }
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ leftMotor = hardwareMap.dcMotor.get("left motor");
+ rightMotor = hardwareMap.dcMotor.get("right motor");
+
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData,
+ NAVX_DEVICE_UPDATE_RATE_HZ);
+
+ rightMotor.setDirection(DcMotor.Direction.REVERSE);
+
+ /* If possible, use encoders when driving, as it results in more */
+ /* predictable drive system response. */
+ //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+ //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+
+ /* Create a PID Controller which uses the Yaw Angle as input. */
+ yawPIDController = new navXPIDController( navx_device,
+ navXPIDController.navXTimestampedDataSource.YAW);
+
+ /* Configure the PID controller */
+ yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
+ yawPIDController.setContinuous(true);
+ yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
+ yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
+ yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
+ yawPIDController.enable(true);
+
+ waitForStart();
+
+ while ( !calibration_complete ) {
+ /* navX-Micro Calibration completes automatically ~15 seconds after it is
+ powered on, as long as the device is still. To handle the case where the
+ navX-Micro has not been able to calibrate successfully, hold off using
+ the navX-Micro Yaw value until calibration is complete.
+ */
+ calibration_complete = !navx_device.isCalibrating();
+ if (!calibration_complete) {
+ telemetry.addData("navX-Micro", "Startup Calibration in Progress");
+ }
+ }
+ navx_device.zeroYaw();
+
+ /* Wait for new Yaw PID output values, then update the motors
+ with the new PID value with each new output value.
+ */
+
+ final double TOTAL_RUN_TIME_SECONDS = 10.0;
+ int DEVICE_TIMEOUT_MS = 500;
+ navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();
+
+ /* Drive straight forward at 1/2 of full drive speed */
+ double drive_speed = 0.5;
+
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ try {
+ while ((runtime.time() < TOTAL_RUN_TIME_SECONDS) &&
+ !Thread.currentThread().isInterrupted()) {
+ if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
+ if (yawPIDResult.isOnTarget()) {
+ leftMotor.setPower(drive_speed);
+ rightMotor.setPower(drive_speed);
+ telemetry.addData("PIDOutput", df.format(drive_speed) + ", " +
+ df.format(drive_speed));
+ } else {
+ double output = yawPIDResult.getOutput();
+ leftMotor.setPower(drive_speed + output);
+ rightMotor.setPower(drive_speed - output);
+ telemetry.addData("PIDOutput", df.format(limit(drive_speed + output)) + ", " +
+ df.format(limit(drive_speed - output)));
+ }
+ telemetry.addData("Yaw", df.format(navx_device.getYaw()));
+ } else{
+ /* A timeout occurred */
+ Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
+ }
+ }
+ }
+ catch(InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ finally {
+ navx_device.close();
+ telemetry.addData("LinearOp", "Complete");
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXDriveStraightPIDLoopOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXDriveStraightPIDLoopOp.java
new file mode 100644
index 00000000000..fac038bff05
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXDriveStraightPIDLoopOp.java
@@ -0,0 +1,177 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.kauailabs.navx.ftc.navXPIDController;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/*
+ * An example loop op mode where the robot will drive in
+ * a straight line (where the driving direction is guided by
+ * the Yaw angle from a navX-Model device).
+ *
+ * This example uses a simple PID controller configuration
+ * with a P coefficient, and will likely need tuning in order
+ * to achieve optimal performance.
+ *
+ * Note that for the best accuracy, a reasonably high update rate
+ * for the navX-Model sensor should be used. This example uses
+ * the default update rate (50Hz), which may be lowered in order
+ * to reduce the frequency of the updates to the drive system.
+ */
+
+@TeleOp(name = "Concept: navX Drive Straight PID - Loop", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXDriveStraightPIDLoopOp extends OpMode {
+ DcMotor leftMotor;
+ DcMotor rightMotor;
+
+ /* This is the port on the Core Device Interface Module */
+ /* in which the navX-Model Device is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+ private AHRS navx_device;
+ private navXPIDController yawPIDController;
+ private ElapsedTime runtime = new ElapsedTime();
+
+ private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50;
+
+ private final double TARGET_ANGLE_DEGREES = 0.0;
+ private final double TOLERANCE_DEGREES = 2.0;
+ private final double MIN_MOTOR_OUTPUT_VALUE = -1.0;
+ private final double MAX_MOTOR_OUTPUT_VALUE = 1.0;
+ private final double YAW_PID_P = 0.005;
+ private final double YAW_PID_I = 0.0;
+ private final double YAW_PID_D = 0.0;
+
+ private boolean calibration_complete = false;
+
+ navXPIDController.PIDResult yawPIDResult;
+ DecimalFormat df;
+
+ @Override
+ public void init() {
+ leftMotor = hardwareMap.dcMotor.get("left motor");
+ rightMotor = hardwareMap.dcMotor.get("right motor");
+
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData,
+ NAVX_DEVICE_UPDATE_RATE_HZ);
+
+ rightMotor.setDirection(DcMotor.Direction.REVERSE);
+
+ /* If possible, use encoders when driving, as it results in more */
+ /* predictable drive system response. */
+ //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+ //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+
+ /* Create a PID Controller which uses the Yaw Angle as input. */
+ yawPIDController = new navXPIDController( navx_device,
+ navXPIDController.navXTimestampedDataSource.YAW);
+
+ /* Configure the PID controller */
+ yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
+ yawPIDController.setContinuous(true);
+ yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
+ yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
+ yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
+ yawPIDController.enable(true);
+
+ df = new DecimalFormat("#.##");
+ }
+
+ public double limit(double a) {
+ return Math.min(Math.max(a, MIN_MOTOR_OUTPUT_VALUE), MAX_MOTOR_OUTPUT_VALUE);
+ }
+
+ @Override
+ public void start() {
+ /* reset the navX-Model device yaw angle so that whatever direction */
+ /* it is currently pointing will be zero degrees. */
+ navx_device.zeroYaw();
+ yawPIDResult = new navXPIDController.PIDResult();
+ }
+
+ @Override
+ public void loop() {
+ if ( !calibration_complete ) {
+ /* navX-Micro Calibration completes automatically ~15 seconds after it is
+ powered on, as long as the device is still. To handle the case where the
+ navX-Micro has not been able to calibrate successfully, hold off using
+ the navX-Micro Yaw value until calibration is complete.
+ */
+ calibration_complete = !navx_device.isCalibrating();
+ if ( calibration_complete ) {
+ navx_device.zeroYaw();
+ } else {
+ telemetry.addData("navX-Micro", "Startup Calibration in Progress");
+ }
+ } else {
+ /* Wait for new Yaw PID output values, then update the motors
+ with the new PID value with each new output value.
+ */
+
+ /* Drive straight forward at 1/2 of full drive speed */
+ double drive_speed = 0.5;
+
+ if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
+ if (yawPIDResult.isOnTarget()) {
+ leftMotor.setPower(drive_speed);
+ rightMotor.setPower(drive_speed);
+ telemetry.addData("Motor Output", df.format(drive_speed) + ", " +
+ df.format(drive_speed));
+ } else {
+ double output = yawPIDResult.getOutput();
+ leftMotor.setPower(limit(drive_speed + output));
+ rightMotor.setPower(limit(drive_speed - output));
+ telemetry.addData("Motor Output", df.format(limit(drive_speed + output)) + ", " +
+ df.format(limit(drive_speed - output)));
+ }
+ } else {
+ /* No sensor update has been received since the last time */
+ /* the loop() function was invoked. Therefore, there's no */
+ /* need to update the motors at this time. */
+ }
+ telemetry.addData("Yaw", df.format(navx_device.getYaw()));
+ }
+ }
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXMotionDetectionOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXMotionDetectionOp.java
new file mode 100644
index 00000000000..b0157682558
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXMotionDetectionOp.java
@@ -0,0 +1,108 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/**
+ * navX-Micro Processed Data Mode Op
+ *
+ * Acquires motion-in-progress indicator from navX-Micro
+ * and displays it in the Robot DriveStation
+ * as telemetry data.
+ */
+@TeleOp(name = "Concept: navX Motion Detection", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXMotionDetectionOp extends OpMode {
+
+ /* This is the port on the Core Device Interace Module */
+ /* in which the navX-Micro is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+
+ private String startDate;
+ private ElapsedTime runtime = new ElapsedTime();
+ private AHRS navx_device;
+
+ @Override
+ public void init() {
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData);
+ }
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+ /*
+ * Code to run when the op mode is first enabled goes here
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init_loop() {
+ telemetry.addData("navX Op Init Loop", runtime.toString());
+ }
+
+ /*
+ * This method will be called repeatedly in a loop
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ */
+ @Override
+ public void loop() {
+
+ boolean connected = navx_device.isConnected();
+ telemetry.addData("1 navX-Device", connected ?
+ "Connected" : "Disconnected" );
+ String gyrocal, motion;
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ if ( connected ) {
+ gyrocal = (navx_device.isCalibrating() ?
+ "CALIBRATING" : "Calibration Complete");
+ motion = (navx_device.isMoving() ? "Moving" : "Not Moving");
+ if ( navx_device.isRotating() ) {
+ motion += ", Rotating";
+ }
+ } else {
+ gyrocal =
+ motion = "-------";
+ }
+ telemetry.addData("2 GyroAccel", gyrocal );
+ telemetry.addData("3 Motion", motion);
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXPerformanceTuningOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXPerformanceTuningOp.java
new file mode 100644
index 00000000000..9275ccbdd57
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXPerformanceTuningOp.java
@@ -0,0 +1,127 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.kauailabs.navx.ftc.navXPerformanceMonitor;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/**
+ * navX-Micro Performance Tuning Op
+ *
+ * This opmode provides insight into the peformance of the communication
+ * with the navX-Model sensor over the I2C bus via the Core Device Interface
+ * Module. Since the Android operating system is not a real-time
+ * operating system, and since communication w/the navX-Model sensor is
+ * occurring over a wifi-direct network which can be prone to interference,
+ * the actual performance (update rate) achieved may be less than
+ * one might expect.
+ *
+ * Since the navX-Model devices integrate sensor data onboard, to achieve
+ * the best performance for device control methods like a PID controller
+ * that work best with constant-time updates, the strategy is to:
+ *
+ * 1) Configure the navX-Model device to a high update rate (e.g., 50Hz)
+ * 2) Using this performance-tuning Op-Mode (with all other
+ * sensors connected, just as your robot will be configured for normal
+ * use) observe the "effective" update rate, and the number of missed
+ * samples over the last minute.
+ * 3) Lower the navX-Model device update rate until the number of missed
+ * samples over the last minute reaches zero.
+ */
+@TeleOp(name = "Concept: navX Performance Tuning", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXPerformanceTuningOp extends OpMode {
+
+ /* This is the port on the Core Device Interface Module */
+ /* in which the navX-Micro is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+
+ private AHRS navx_device;
+ private navXPerformanceMonitor navx_perfmon;
+ private byte sensor_update_rate_hz = 40;
+ private ElapsedTime runtime = new ElapsedTime();
+
+ @Override
+ public void init() {
+ AHRS.setLogging(true);
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData,
+ sensor_update_rate_hz);
+ navx_perfmon = new navXPerformanceMonitor(navx_device);
+ }
+
+@Override
+ public void start() {
+ navx_device.registerCallback(navx_perfmon);
+}
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+ /*
+ * Code to run when the op mode is first enabled goes here
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init_loop() {
+ telemetry.addData("navX Op Init Loop", runtime.toString());
+ }
+
+ /*
+ * This method will be called repeatedly in a loop
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ */
+ @Override
+ public void loop() {
+
+ boolean connected = navx_device.isConnected();
+ telemetry.addData("1 navX-Device...............:", connected ?
+ "Connected" : "Disconnected" );
+ String gyrocal, motion;
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ telemetry.addData("2 Sensor Rate (Hz)...", Byte.toString(navx_device.getActualUpdateRate()));
+ telemetry.addData("3 Transfer Rate (Hz).", Integer.toString(navx_device.getCurrentTransferRate()));
+ telemetry.addData("4 Delivvered Rate (Hz)", Integer.toString(navx_perfmon.getDeliveredRateHz()));
+ telemetry.addData("5 Missed Samples.....", Integer.toString(navx_perfmon.getNumMissedSensorTimestampedSamples()));
+ telemetry.addData("6 Duplicate Samples..", Integer.toString(navx_device.getDuplicateDataCount()));
+ telemetry.addData("7 Sensor deltaT (ms).", Long.toString(navx_perfmon.getLastSensorTimestampDeltaMS()));
+ telemetry.addData("8 System deltaT (ms).", Long.toString(navx_perfmon.getLastSystemTimestampDeltaMS()));
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXRotateToAnglePIDLinearOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXRotateToAnglePIDLinearOp.java
new file mode 100644
index 00000000000..f8c7fd9d1a2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXRotateToAnglePIDLinearOp.java
@@ -0,0 +1,166 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import android.util.Log;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.kauailabs.navx.ftc.navXPIDController;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/*
+ * An example linear op mode where the robot will rotate
+ * to a specified angle an then stop.
+ *
+ * This example uses a simple PID controller configuration
+ * with a P coefficient, and will likely need tuning in order
+ * to achieve optimal performance.
+ *
+ * Note that for the best accuracy, a reasonably high update rate
+ * for the navX-Model sensor should be used.
+ */
+@TeleOp(name = "Concept: navX Rotate to Angle PID - Linear", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXRotateToAnglePIDLinearOp extends LinearOpMode {
+ DcMotor leftMotor;
+ DcMotor rightMotor;
+
+ /* This is the port on the Core Device Interface Module */
+ /* in which the navX-Model Device is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+ private AHRS navx_device;
+ private navXPIDController yawPIDController;
+ private ElapsedTime runtime = new ElapsedTime();
+
+ private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50;
+
+ private final double TARGET_ANGLE_DEGREES = 90.0;
+ private final double TOLERANCE_DEGREES = 2.0;
+ private final double MIN_MOTOR_OUTPUT_VALUE = -1.0;
+ private final double MAX_MOTOR_OUTPUT_VALUE = 1.0;
+ private final double YAW_PID_P = 0.005;
+ private final double YAW_PID_I = 0.0;
+ private final double YAW_PID_D = 0.0;
+
+ private boolean calibration_complete = false;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ leftMotor = hardwareMap.dcMotor.get("left motor");
+ rightMotor = hardwareMap.dcMotor.get("right motor");
+
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData,
+ NAVX_DEVICE_UPDATE_RATE_HZ);
+
+ rightMotor.setDirection(DcMotor.Direction.REVERSE);
+
+ /* If possible, use encoders when driving, as it results in more */
+ /* predictable drive system response. */
+ //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+ //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+
+ /* Create a PID Controller which uses the Yaw Angle as input. */
+ yawPIDController = new navXPIDController( navx_device,
+ navXPIDController.navXTimestampedDataSource.YAW);
+
+ /* Configure the PID controller */
+ yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
+ yawPIDController.setContinuous(true);
+ yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
+ yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
+ yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
+
+ waitForStart();
+
+ while ( !calibration_complete ) {
+ /* navX-Micro Calibration completes automatically ~15 seconds after it is
+ powered on, as long as the device is still. To handle the case where the
+ navX-Micro has not been able to calibrate successfully, hold off using
+ the navX-Micro Yaw value until calibration is complete.
+ */
+ calibration_complete = !navx_device.isCalibrating();
+ if (!calibration_complete) {
+ telemetry.addData("navX-Micro", "Startup Calibration in Progress");
+ }
+ }
+ navx_device.zeroYaw();
+
+ try {
+ yawPIDController.enable(true);
+
+ /* Wait for new Yaw PID output values, then update the motors
+ with the new PID value with each new output value.
+ */
+
+ final double TOTAL_RUN_TIME_SECONDS = 30.0;
+ int DEVICE_TIMEOUT_MS = 500;
+ navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();
+
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ while ( (runtime.time() < TOTAL_RUN_TIME_SECONDS) &&
+ !Thread.currentThread().isInterrupted()) {
+ if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
+ if (yawPIDResult.isOnTarget()) {
+ leftMotor.setPowerFloat();
+ rightMotor.setPowerFloat();
+ telemetry.addData("PIDOutput", df.format(0.00));
+ } else {
+ double output = yawPIDResult.getOutput();
+ leftMotor.setPower(output);
+ rightMotor.setPower(-output);
+ telemetry.addData("PIDOutput", df.format(output) + ", " +
+ df.format(-output));
+ }
+ } else {
+ /* A timeout occurred */
+ Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
+ }
+ telemetry.addData("Yaw", df.format(navx_device.getYaw()));
+ }
+ }
+ catch(InterruptedException ex) {
+ Thread.currentThread().interrupt();
+ }
+ finally {
+ navx_device.close();
+ telemetry.addData("LinearOp", "Complete");
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXRotateToAnglePIDLoopOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXRotateToAnglePIDLoopOp.java
new file mode 100644
index 00000000000..29a395826fb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXRotateToAnglePIDLoopOp.java
@@ -0,0 +1,162 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.kauailabs.navx.ftc.navXPIDController;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/*
+ * An example loop op mode where the robot will rotate
+ * to a specified angle an then stop.
+ *
+ * This example uses a simple PID controller configuration
+ * with a P coefficient, and will likely need tuning in order
+ * to achieve optimal performance.
+ *
+ * Note that for the best accuracy, a reasonably high update rate
+ * for the navX-Model sensor should be used.
+ */
+@TeleOp(name = "Concept: navX Rotate to Angle PID - Loop", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXRotateToAnglePIDLoopOp extends OpMode {
+ DcMotor leftMotor;
+ DcMotor rightMotor;
+
+ /* This is the port on the Core Device Interface Module */
+ /* in which the navX-Model Device is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+ private AHRS navx_device;
+ private navXPIDController yawPIDController;
+ private ElapsedTime runtime = new ElapsedTime();
+
+ private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50;
+
+ private final double TARGET_ANGLE_DEGREES = 90.0;
+ private final double TOLERANCE_DEGREES = 2.0;
+ private final double MIN_MOTOR_OUTPUT_VALUE = -1.0;
+ private final double MAX_MOTOR_OUTPUT_VALUE = 1.0;
+ private final double YAW_PID_P = 0.005;
+ private final double YAW_PID_I = 0.0;
+ private final double YAW_PID_D = 0.0;
+
+ private boolean calibration_complete = false;
+
+ navXPIDController.PIDResult yawPIDResult;
+ DecimalFormat df;
+
+ @Override
+ public void init() {
+ leftMotor = hardwareMap.dcMotor.get("left motor");
+ rightMotor = hardwareMap.dcMotor.get("right motor");
+
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData,
+ NAVX_DEVICE_UPDATE_RATE_HZ);
+
+ rightMotor.setDirection(DcMotor.Direction.REVERSE);
+
+ /* If possible, use encoders when driving, as it results in more */
+ /* predictable drive system response. */
+ //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+ //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
+
+ /* Create a PID Controller which uses the Yaw Angle as input. */
+ yawPIDController = new navXPIDController( navx_device,
+ navXPIDController.navXTimestampedDataSource.YAW);
+
+ /* Configure the PID controller */
+ yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
+ yawPIDController.setContinuous(true);
+ yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
+ yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
+ yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
+ yawPIDController.enable(true);
+
+ df = new DecimalFormat("#.##");
+ }
+
+ @Override
+ public void start() {
+ navx_device.zeroYaw();
+ yawPIDResult = new navXPIDController.PIDResult();
+ }
+
+ @Override
+ public void loop() {
+ if ( !calibration_complete ) {
+ /* navX-Micro Calibration completes automatically ~15 seconds after it is
+ powered on, as long as the device is still. To handle the case where the
+ navX-Micro has not been able to calibrate successfully, hold off using
+ the navX-Micro Yaw value until calibration is complete.
+ */
+ calibration_complete = !navx_device.isCalibrating();
+ if ( calibration_complete ) {
+ navx_device.zeroYaw();
+ } else {
+ telemetry.addData("navX-Micro", "Startup Calibration in Progress");
+ }
+ } else {
+ /* Wait for new Yaw PID output values, then update the motors
+ with the new PID value with each new output value.
+ */
+ if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
+ if (yawPIDResult.isOnTarget()) {
+ leftMotor.setPowerFloat();
+ rightMotor.setPowerFloat();
+ telemetry.addData("Motor Output", df.format(0.00));
+ } else {
+ double output = yawPIDResult.getOutput();
+ leftMotor.setPower(output);
+ rightMotor.setPower(-output);
+ telemetry.addData("Motor Output", df.format(output) + ", " +
+ df.format(-output));
+ }
+ } else {
+ /* No sensor update has been received since the last time */
+ /* the loop() function was invoked. Therefore, there's no */
+ /* need to update the motors at this time. */
+ }
+ telemetry.addData("Yaw", df.format(navx_device.getYaw()));
+ }
+ }
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXZeroYawOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXZeroYawOp.java
new file mode 100644
index 00000000000..608b654c458
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/ConceptNavXZeroYawOp.java
@@ -0,0 +1,147 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/**
+ * navX-Micro Zero Yaw Op
+ *
+ * Acquires processed data from navX-Micro
+ * and displays it in the Robot DriveStation
+ * as telemetry data. This opmode demonstrates how to "zero"
+ * (reset to zero degrees) the yaw. This causes whatever
+ * direction the navX-Model device is currently pointing to
+ * now be zero degrees, and is an effective method for dealing
+ * with accumulating Yaw Drift.
+ *
+ * For more information on Yaw drift, please see the online help at:
+ * http://navx-micro.kauailabs.com/guidance/yaw-drift/
+ */
+@TeleOp(name = "Concept: navX Zero Yaw", group = "Concept")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class ConceptNavXZeroYawOp extends OpMode {
+
+ /* This is the port on the Core Device Interace Module */
+ /* in which the navX-Micro is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 0;
+
+ private String startDate;
+ private ElapsedTime runtime = new ElapsedTime();
+ private AHRS navx_device;
+
+ @Override
+ public void init() {
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData);
+ }
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+ /*
+ * Code to run when the op mode is first enabled goes here
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init_loop() {
+ telemetry.addData("navX Op Init Loop", runtime.toString());
+ }
+
+ /*
+ * This method will be called repeatedly in a loop
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ */
+ @Override
+ public void loop() {
+
+ boolean connected = navx_device.isConnected();
+ telemetry.addData("1 navX-Device", connected ?
+ "Connected" : "Disconnected" );
+ String gyrocal, magcal, yaw, pitch, roll, compass_heading;
+ String fused_heading, ypr, cf, motion;
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ if ( connected ) {
+ gyrocal = (navx_device.isCalibrating() ?
+ "CALIBRATING" : "Calibration Complete");
+ magcal = (navx_device.isMagnetometerCalibrated() ?
+ "Calibrated" : "UNCALIBRATED");
+ yaw = df.format(navx_device.getYaw());
+ pitch = df.format(navx_device.getPitch());
+ roll = df.format(navx_device.getRoll());
+ ypr = yaw + ", " + pitch + ", " + roll;
+ compass_heading = df.format(navx_device.getCompassHeading());
+ fused_heading = df.format(navx_device.getFusedHeading());
+ if (!navx_device.isMagnetometerCalibrated()) {
+ compass_heading = "-------";
+ }
+ cf = compass_heading + ", " + fused_heading;
+ if ( navx_device.isMagneticDisturbance()) {
+ cf += " (Mag. Disturbance)";
+ }
+ motion = (navx_device.isMoving() ? "Moving" : "Not Moving");
+ if ( navx_device.isRotating() ) {
+ motion += ", Rotating";
+ }
+ } else {
+ gyrocal =
+ magcal =
+ ypr =
+ cf =
+ motion = "-------";
+ }
+ telemetry.addData("2 GyroAccel", gyrocal );
+ telemetry.addData("3 Y,P,R", ypr);
+ telemetry.addData("4 Magnetometer", magcal );
+ telemetry.addData("5 Compass,9Axis", cf );
+ telemetry.addData("6 Motion", motion);
+
+ /* If the left 'bumper' button pressed,
+ reset (zero) the current yaw angle. This causes whatever
+ direction the navX-Model device is currently pointing to
+ now be zero degrees.
+ */
+ //if ( gamepad1.left_bumper ) {
+ if ( ( navx_device.getUpdateCount() % 500 ) == 0 ) {
+ navx_device.zeroYaw();
+ }
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/SensorNavXProcessedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/SensorNavXProcessedOp.java
new file mode 100644
index 00000000000..7918054c891
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/SensorNavXProcessedOp.java
@@ -0,0 +1,133 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/**
+ * navX-Micro Processed Data Mode Op
+ *
+ * Acquires processed data from navX-Micro
+ * and displays it in the Robot DriveStation
+ * as telemetry data. This processed data includes
+ * Yaw, Pitch, Roll, Compass Heading, Fused (9-axis) Heading,
+ * Sensor Status and Timestamp, and World-Frame Linear
+ * Acceleration data.
+ */
+@TeleOp(name = "Sensor: navX Motion-processed Data", group = "Sensor")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class SensorNavXProcessedOp extends OpMode {
+
+ /* This is the port on the Core Device Interace Module */
+ /* in which the navX-Micro is connected. Modify this */
+ /* depending upon which I2C port you are using. */
+ private final int NAVX_DIM_I2C_PORT = 5;
+
+ private String startDate;
+ private ElapsedTime runtime = new ElapsedTime();
+ private AHRS navx_device;
+
+ @Override
+ public void init() {
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kProcessedData);
+ }
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+ /*
+ * Code to run when the op mode is first enabled goes here
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init_loop() {
+ telemetry.addData("navX Op Init Loop", runtime.toString());
+ }
+
+ /*
+ * This method will be called repeatedly in a loop
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ */
+ @Override
+ public void loop() {
+
+ boolean connected = navx_device.isConnected();
+ telemetry.addData("1 navX-Device", connected ?
+ "Connected" : "Disconnected" );
+ String gyrocal, magcal, yaw, pitch, roll, compass_heading;
+ String fused_heading, ypr, cf, motion;
+ DecimalFormat df = new DecimalFormat("#.##");
+
+ if ( connected ) {
+ gyrocal = (navx_device.isCalibrating() ?
+ "CALIBRATING" : "Calibration Complete");
+ magcal = (navx_device.isMagnetometerCalibrated() ?
+ "Calibrated" : "UNCALIBRATED");
+ yaw = df.format(navx_device.getYaw());
+ pitch = df.format(navx_device.getPitch());
+ roll = df.format(navx_device.getRoll());
+ ypr = yaw + ", " + pitch + ", " + roll;
+ compass_heading = df.format(navx_device.getCompassHeading());
+ fused_heading = df.format(navx_device.getFusedHeading());
+ if (!navx_device.isMagnetometerCalibrated()) {
+ compass_heading = "-------";
+ }
+ cf = compass_heading + ", " + fused_heading;
+ if ( navx_device.isMagneticDisturbance()) {
+ cf += " (Mag. Disturbance)";
+ }
+ motion = (navx_device.isMoving() ? "Moving" : "Not Moving");
+ if ( navx_device.isRotating() ) {
+ motion += ", Rotating";
+ }
+ } else {
+ gyrocal =
+ magcal =
+ ypr =
+ cf =
+ motion = "-------";
+ }
+ telemetry.addData("2 GyroAccel", gyrocal );
+ telemetry.addData("3 Y,P,R", ypr);
+ telemetry.addData("4 Magnetometer", magcal );
+ telemetry.addData("5 Compass,9Axis", cf );
+ telemetry.addData("6 Motion", motion);
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/SensorNavXRawOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/SensorNavXRawOp.java
new file mode 100644
index 00000000000..82030601c75
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/navx/SensorNavXRawOp.java
@@ -0,0 +1,141 @@
+/* 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 org.firstinspires.ftc.teamcode.opmodes.navx;
+
+import com.kauailabs.navx.ftc.AHRS;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.text.DecimalFormat;
+
+/**
+ * SensorNavXRawOp
+ *
+ * This sample demonstrates how to acquire the raw
+ * Gyroscope, Accelerometer and Magnetometer data. This raw
+ * data is typically not as useful as the "processed" data
+ * (see the navXProcessedOp for details), however is provided
+ * for those interested in accessing the raw data.
+ *
+ * Gyroscope data is units of Degrees/second.
+ * Accelerometer data is in units of G.
+ * Magnetometer data is in units if microTorr (uT)
+ *
+ * Magnetometer data is not valid unless magnetometer calibration
+ * has been performed. Without calibration, the magnetometer
+ * data will be all zeros. For details on how to calibrate the
+ * magnetometer, please see the Magnetometer Calibration documentation:
+ * http://navx-micro.kauailabs.com/guidance/magnetometer-calibration/
+ *
+ * Note that due to limitations imposed by the Core Device
+ * Interface Module's I2C interface mechanisms, the acquisition
+ * of both processed data and raw data requires two separate
+ * I2C bus transfers, and thus takes longer than acquiring
+ * only the raw or only the processed data.
+ */
+@TeleOp(name = "Sensor: navX Raw Data", group = "Sensor")
+@Disabled //Comment this in to remove this from the Driver Station OpMode List
+public class SensorNavXRawOp extends OpMode {
+
+ private final int NAVX_DIM_I2C_PORT = 5;
+ private String startDate;
+ private ElapsedTime runtime = new ElapsedTime();
+ private AHRS navx_device;
+ @Override
+ public void init() {
+ navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"),
+ NAVX_DIM_I2C_PORT,
+ AHRS.DeviceDataType.kQuatAndRawData); }
+
+ @Override
+ public void stop() {
+ navx_device.close();
+ }
+ /*
+ * Code to run when the op mode is first enabled goes here
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init_loop() {
+ telemetry.addData("navX Op Init Loop", runtime.toString());
+ }
+
+ /*
+ * This method will be called repeatedly in a loop
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ */
+
+ @Override
+ public void start(){
+ navx_device.zeroYaw();
+ }
+
+ @Override
+ public void loop() {
+
+ boolean connected = navx_device.isConnected();
+ telemetry.addData("1 navX-Device", connected ? "Connected" : "Disconnected" );
+ String gyrocal, gyro_raw, accel_raw, mag_raw, quat_raw;
+ boolean magnetometer_calibrated;
+ if ( connected ) {
+ DecimalFormat df = new DecimalFormat("#.##");
+ magnetometer_calibrated = navx_device.isMagnetometerCalibrated();
+ gyro_raw = df.format(navx_device.getRawGyroX()) + ", " +
+ df.format(navx_device.getRawGyroY()) + ", " +
+ df.format(navx_device.getRawGyroZ());
+ accel_raw = df.format(navx_device.getRawAccelX()) + ", " +
+ df.format(navx_device.getRawAccelY()) + ", " +
+ df.format(navx_device.getRawAccelZ());
+ quat_raw = df.format(navx_device.getQuaternionX()) + ", " +
+ df.format(navx_device.getQuaternionY()) + ", " +
+ df.format(navx_device.getQuaternionZ()) + ", " +
+ df.format(navx_device.getQuaternionW());
+ if ( magnetometer_calibrated ) {
+ mag_raw = df.format(navx_device.getRawMagX()) + ", " +
+ df.format(navx_device.getRawMagY()) + ", " +
+ df.format(navx_device.getRawMagZ());
+ } else {
+ mag_raw = "Uncalibrated";
+ }
+ } else {
+ gyro_raw =
+ accel_raw =
+ quat_raw =
+ mag_raw = "-------";
+ }
+ telemetry.addData("2 Gyros (Degrees/Sec):", gyro_raw);
+ telemetry.addData("3 Accelerometers (G):", accel_raw );
+ telemetry.addData("4 Magnetometers (uT):", mag_raw );
+ telemetry.addData("5 Quaternions (?)", quat_raw);
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/neural/PictoClassify.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/neural/PictoClassify.java
new file mode 100644
index 00000000000..b4a13df0fb6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/neural/PictoClassify.java
@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.opmodes.neural;
+
+/**
+ * Created by Noah on 2/9/2018.
+ * Simple pictogram classifier based on a neural netowrk build in Keras
+ * Designed to be called on asynchronously, since NN calculations are expensive
+ * Both OpenCV and Tensorflow must be initialized to use any of these functions!
+ */
+
+public class PictoClassify {
+ int[] cropSize;
+ int[] scaleSize;
+
+ public PictoClassify(String modelFPath, int[] cropSize, int[] scaleSize) {
+ this.cropSize = cropSize;
+ this.scaleSize = scaleSize;
+ //load model file from disk
+
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/ADPSAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/ADPSAuto.java
new file mode 100644
index 00000000000..be064194a54
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/ADPSAuto.java
@@ -0,0 +1,525 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2018;
+
+import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+import com.vuforia.CameraDevice;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
+import org.firstinspires.ftc.teamcode.libraries.GyroCorrectStep;
+import org.firstinspires.ftc.teamcode.libraries.UltraHoneStep;
+import org.firstinspires.ftc.teamcode.libraries.hardware.APDS9930;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.FilterLib;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.VuforiaBallLib;
+import org.firstinspires.ftc.teamcode.libraries.hardware.APDS9960;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BlinkyEffect;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+import org.firstinspires.ftc.teamcode.libraries.hardware.MatbotixUltra;
+import org.firstinspires.ftc.teamcode.libraries.hardware.StupidColor;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.HeadingSensor;
+import org.firstinspires.ftc.teamcode.opmodes.demo.Color;
+import org.firstinspires.ftc.teamcode.opmodes.diagnostic.UltraHoneDebug;
+
+import java.util.Arrays;
+import java.util.LinkedList;
+import java.util.concurrent.TimeUnit;
+
+/**
+ * Created by Noah on 12/20/2017.
+ * Take a guess
+ */
+
+@Autonomous(name="Blue Front Auto", group="test")
+public class ADPSAuto extends VuforiaBallLib {
+ private static final double BALL_WAIT_SEC = 2.0;
+ private static final int UNDERGLOW_PULSE_WAIT = 50;
+ private static final double UNDERGLOW_INC = 0.01;
+ private static final double UNDERGLOW_MAX = 0.5;
+
+ protected APDS9960 backDist; //formerly redDist
+ protected final APDS9960.Config backConfig = new APDS9960.Config();
+ protected APDS9960 frontDist;
+ protected final APDS9960.Config frontConfig = new APDS9960.Config();
+ protected MatbotixUltra frontUltra;
+ protected MatbotixUltra backUltra;
+ protected boolean red = false;
+ protected boolean rear = false;
+ private BotHardware bot = new BotHardware(this);
+
+ private BallColor color = BallColor.Undefined;
+ private BallColor altColor = BallColor.Undefined;
+ private double startLoop = 0;
+ private boolean firstLoop = false;
+
+ private AutoLib.Sequence mSeq = new AutoLib.LinearSequence();
+
+ //parameters for gyro steering
+ private static final float Kp4 = -8.0f; // degree heading proportional term correction per degree of deviation
+ private static final float Ki4 = 0.0f; // ... integrator term
+ private static final float Kd4 = 0; // ... derivative term
+ private static final float Ki4Cutoff = 0.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID drivePID = new SensorLib.PID(Kp4, Ki4, Kd4, Ki4Cutoff);
+
+ //parameters for gyro turning
+ private static final float Kp5 = 10.0f; // degree heading proportional term correction per degree of deviation
+ private static final float Ki5 = 0.0f; // ... integrator term
+ private static final float Kd5 = 0; // ... derivative term
+ private static final float Ki5Cutoff = 0.0f; // maximum angle error for which we update integrator
+
+ private final SensorLib.PID motorPID = new SensorLib.PID(Kp5, Ki5, Kd5, Ki5Cutoff);
+
+ private static final double MM_PER_ENCODE = 13.298;
+ private static final int APDS_DIST = 60;
+
+ private boolean glowInc = true;
+ private double glowPower = 0.0;
+ private long glowLastTime;
+
+ private enum AutoPath {
+ RED_FRONT_LEFT(true, false, RelicRecoveryVuMark.LEFT, 1, 125, 720, 32),
+ RED_FRONT_CENTER(true, false, RelicRecoveryVuMark.CENTER, 0, 125, 680, 32),
+ RED_FRONT_RIGHT(true, false, RelicRecoveryVuMark.RIGHT, 2, 55, 740, 32),
+ RED_REAR_LEFT(true, true, RelicRecoveryVuMark.LEFT, 1, 125, 590, 33),
+ RED_REAR_CENTER(true, true, RelicRecoveryVuMark.CENTER, 0, 125, 540, 33),
+ RED_REAR_RIGHT(true, true, RelicRecoveryVuMark.RIGHT, 2, 50, 640, 33),
+
+ BLUE_FRONT_LEFT(false, false, RelicRecoveryVuMark.LEFT, 3, 130, 700, 32),
+ BLUE_FRONT_CENTER(false, false, RelicRecoveryVuMark.CENTER, 1, 55, 600, 32),
+ BLUE_FRONT_RIGHT(false, false, RelicRecoveryVuMark.RIGHT, 2, 55, 640, 32),
+ BLUE_REAR_LEFT(false, true, RelicRecoveryVuMark.LEFT, 1, 115, 320, 33),
+ BLUE_REAR_CENTER(false, true, RelicRecoveryVuMark.CENTER, 0, 55, 520, 33),
+ BLUE_REAR_RIGHT(false, true, RelicRecoveryVuMark.RIGHT, 1, 55, 520, 33);
+
+ //robot and field constants (in cm)
+ private final double REAR_PILLAR_WALL_DIST = DistanceUnit.INCH.toCm(24);
+ private final double FRONT_PILLAR_WALL_DIST = DistanceUnit.INCH.toCm(48);
+ private final double PILLAR_COL_DIST = DistanceUnit.INCH.toCm(7.63);
+ private final double RED_WHACKY_TO_ULTRA = 12.265;
+ private final double BLUE_WHACKY_TO_ULTRA = 30.591;
+
+ public final RelicRecoveryVuMark vuMark;
+ public final boolean red;
+ public final boolean rear;
+ public final int skipCount;
+ public final float turnAmount;
+ public final int driveCounts;
+ public final int ultraDist;
+ public final int apdsFailUltra;
+ AutoPath(boolean red, boolean rear, RelicRecoveryVuMark mark, int skipCount, float turnAmount, int driveCounts, int ultraDist) {
+ this.red = red;
+ this.rear = rear;
+ this.vuMark = mark;
+ this.skipCount = skipCount;
+ this.turnAmount = turnAmount;
+ this.driveCounts = driveCounts;
+ this.ultraDist = ultraDist;
+ //calculate the fallback ultrasonic distance based on known values on the robot and field
+ double fieldDist = this.rear ? REAR_PILLAR_WALL_DIST : FRONT_PILLAR_WALL_DIST;
+ double whackyUltra = this.red ? RED_WHACKY_TO_ULTRA : BLUE_WHACKY_TO_ULTRA;
+ double skipDist = this.skipCount * PILLAR_COL_DIST;
+ this.apdsFailUltra = (int)Math.round(fieldDist + skipDist*skipCount - DistanceUnit.MM.toCm(APDS_DIST) - whackyUltra);
+ }
+
+ static AutoPath getPath(boolean red, boolean rear, RelicRecoveryVuMark mark) {
+ //return match
+ if(mark != null && mark != RelicRecoveryVuMark.UNKNOWN){
+ for(AutoPath path : values()) if(path.red == red && path.rear == rear && path.vuMark == mark) return path;
+ throw new IllegalArgumentException("WTF Why");
+ }
+ //else return default
+ if(red && rear) return RED_REAR_RIGHT;
+ if(red) return RED_FRONT_CENTER;
+ if(rear) return BLUE_REAR_CENTER;
+ return BLUE_FRONT_CENTER;
+ }
+ }
+
+ public void init() {
+ initVuforia(true);
+
+ backDist = new APDS9960(backConfig, hardwareMap.get(I2cDeviceSynch.class, "reddist"));
+ frontDist = new APDS9960(frontConfig, hardwareMap.get(I2cDeviceSynch.class, "bluedist"));
+ frontUltra = new MatbotixUltra(hardwareMap.get(I2cDeviceSynch.class, "ultrafront"), 100);
+ backUltra = new MatbotixUltra(hardwareMap.get(I2cDeviceSynch.class, "ultraback"), 100);
+
+ backDist.initDevice();
+ frontDist.initDevice();
+ frontUltra.initDevice();
+ backUltra.initDevice();
+
+ backDist.startDevice();
+ frontDist.startDevice();
+ frontUltra.startDevice();
+ backUltra.startDevice();
+
+ bot.init();
+ bot.start();
+ bot.setDropPos(0.62);
+
+ telemetry.update();
+
+ startTracking();
+ }
+
+ public void init_loop() {
+ telemetry.addData("Front Ultra", frontUltra.getReading());
+ telemetry.addData("Back Ultra", backUltra.getReading());
+ telemetry.addData("Front Infrared", frontDist.getDist());
+ telemetry.addData("Back Infrared", backDist.getDist());
+ telemetry.addData("Ball Color", getBallColor().toString());
+ telemetry.addData("IMU", bot.getHeadingSensor().getHeading());
+ telemetry.addData("Lift", BotHardware.Motor.lift.motor.getCurrentPosition());
+ }
+
+ public void start() {
+
+ }
+
+ public void loop() {
+ if(startLoop == 0) startLoop = getRuntime();
+ if(getRuntime() - startLoop >= BALL_WAIT_SEC / 2 && !firstLoop) CameraDevice.getInstance().setFlashTorchMode(true);
+ if(getRuntime() - startLoop < BALL_WAIT_SEC && (color == BallColor.Indeterminate || color == BallColor.Undefined)) {
+ color = getBallColor();
+ altColor = getCVBallColor();
+ }
+ else if(!firstLoop){
+ CameraDevice.getInstance().setFlashTorchMode(false);
+ //init whacky stick code here
+ AutoLib.Sequence whack = new AutoLib.LinearSequence();
+
+ if(red) whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed, 0.25, false));
+ else whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue, 0.25, false));
+ whack.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickDown, 0.5, false));
+
+ //hmmmmm
+ final AutoLib.Step whackLeft;
+ if(red) whackLeft = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed - BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ else whackLeft = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue - BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ final AutoLib.Step whackRight;
+ if(red) whackRight = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed + BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ else whackRight = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue + BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ whack.add(new APDSBallFind(red, (color != BallColor.Indeterminate && color != BallColor.Undefined) ? color : altColor, whackLeft, whackRight));
+
+ whack.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickUp, 0.25, false));
+ whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseHidden, 0.25, false));
+
+ final int mul = red ? -1 : 1;
+
+ AutoPath path = AutoPath.getPath(red, rear, getLastVuMark());
+
+ AutoLib.Sequence findPilliar = new AutoLib.LinearSequence();
+
+ GyroCorrectStep step;
+ if(!red) step = new GyroCorrectStep(this, 0, bot.getHeadingSensor(), new SensorLib.PID(-20, 0, 0, 0), bot.getMotorVelocityShimArray(), -250.0f, 45.0f, 360.0f);
+ else step = new GyroCorrectStep(this, 0, bot.getHeadingSensor(), new SensorLib.PID(-20, 0, 0, 0), bot.getMotorVelocityShimArray(), 250.0f, 45.0f, 360.0f);
+
+ int heading = rear ? (red ? 90 : -90) : 0;
+
+ if(!rear) {
+ findPilliar.add(new AutoLib.AzimuthCountedDriveStep(this, 0, bot.getHeadingSensor(), drivePID, bot.getMotorVelocityShimArray(), 250.0f * mul, 1200, true, -360.0f, 360.0f));
+ findPilliar.add(new AutoLib.GyroTurnStep(this, 0, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 45.0f, 360.0f, motorPID, 0.5f, 10, true));
+ }
+ else {
+ //TODO: implement encoder count backup automatically
+ findPilliar.add(new UltraHoneStep(this, red ? frontUltra : backUltra, path.ultraDist, 0, 5, new SensorLib.PID(11f, 0.15f, 0, 10), step));
+ findPilliar.add(new AutoLib.GyroTurnStep(this, heading, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 45.0f, 360.0f, motorPID, 0.5f, 10, true));
+ //findPilliar.add(new AutoLib.GyroTurnStep(this, heading, bot.getHeadingSensor(), bot.getMotorRay(), 0.04f, 0.4f, new SensorLib.PID(0.006f, 0, 0, 0), 0.5f, 10, true));
+ //if(!red) findPilliar.add(new AutoLib.AzimuthCountedDriveStep(this, heading, bot.getHeadingSensor(), drivePID, bot.getMotorVelocityShimArray(), -360.0f, 100, true, -500.0f, 500.0f));
+ }
+
+ findPilliar.add(new AutoLib.TimedServoStep(BotHardware.ServoE.stickBase.servo, BotHardware.ServoE.stickBaseCenter, 0.25, false));
+ findPilliar.add(new AutoLib.TimedServoStep(BotHardware.ServoE.stick.servo, 0.85, 0.25, false));
+
+ //reconstruct for saftey
+ if(!red) step = new GyroCorrectStep(this, heading, bot.getHeadingSensor(), new SensorLib.PID(-30, 0, 0, 0), bot.getMotorVelocityShimArray(), 250.0f, 25.0f, 150.0f);
+ else step = new GyroCorrectStep(this, heading, bot.getHeadingSensor(), new SensorLib.PID(-30, 0, 0, 0), bot.getMotorVelocityShimArray(), -250.0f, 25.0f, 150.0f);
+ final APDS9960 dist = red ? frontDist : backDist;
+ //TODO: add camera fallback
+ findPilliar.add(new APDSFind(BotHardware.ServoE.stick.servo, 0.85, 0.55, dist, new SensorLib.PID(1.0f, 0, 0, 10), step,
+ APDS_DIST, 8, path.skipCount, 100, this, red, rear));
+ findPilliar.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickUp, 0.25, false));
+ findPilliar.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseHidden, 0.25, false));
+ AutoLib.ConcurrentSequence raiseWhile = new AutoLib.ConcurrentSequence();
+ AutoLib.LinearSequence driveToBox = new AutoLib.LinearSequence();
+ //findPilliar.add(new AutoLib.GyroTurnStep(this, path.turnAmount + heading, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 90.0f, 520.0f, motorPID, 2.0f, 5, true));
+ driveToBox.add(new AutoLib.GyroTurnStep(this, path.turnAmount + heading, bot.getHeadingSensor(), bot.getMotorRay(), 0.04f, 0.4f, new SensorLib.PID(0.006f, 0, 0, 0), 2f, 5, true));
+ driveToBox.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 250.0f, path.driveCounts, true));
+ raiseWhile.add(driveToBox);
+ int liftPos = BotHardware.Motor.lift.motor.getCurrentPosition();
+ raiseWhile.add(new MoveToPositionStep(BotHardware.Motor.lift.motor, liftPos + 400, 10, 1.0f, true));
+ findPilliar.add(raiseWhile);
+ findPilliar.add(bot.getDropStep());
+ findPilliar.add(bot.getReverseDropStep());
+ AutoLib.ConcurrentSequence oneSideSeq = new AutoLib.ConcurrentSequence();
+ DcMotor[] temp = bot.getMotorRay();
+ if(path.turnAmount > 55) {
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[0], 0.5f, 1.0, true));
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[1], 0.5f, 1.0, true));
+ }
+ else {
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[2], 0.5f, 1.0, true));
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[3], 0.5f, 1.0, true));
+ }
+ oneSideSeq.add(new MoveToPositionStep(BotHardware.Motor.lift.motor, liftPos, 3, 1.0f, true));
+ findPilliar.add(oneSideSeq);
+ findPilliar.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), -400.0f, 600, true));
+ findPilliar.add(new AutoLib.GyroTurnStep(this,heading + 180, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 90.0f, 520.0f, motorPID, 10.0f, 10, true));
+
+ mSeq.add(whack);
+ mSeq.add(findPilliar);
+
+ firstLoop = true;
+ }
+
+ //logs!
+ if(color != null) telemetry.addData("Ball Color", color.toString());
+ if(getLastVuMark() != null) telemetry.addData("VuMark", getLastVuMark().toString());
+
+ try {
+ if(firstLoop && mSeq.loop()) requestOpModeStop();
+ }
+ catch (Exception e) {
+ backDist.stopDevice();
+ frontDist.stopDevice();
+ bot.stopAll();
+ throw e;
+ }
+ }
+
+
+ public void stop() {
+ backDist.stopDevice();
+ frontDist.stopDevice();
+ backUltra.stopDevice();
+ frontUltra.stopDevice();
+ bot.stopAll();
+ stopVuforia();
+ }
+
+ public static class APDSFind extends AutoLib.Step {
+ private final int mError;
+ private final int mDist;
+ private final APDS9960 sens;
+ private final OpMode mode;
+ private final SensorLib.PID errorPid;
+ private final GyroCorrectStep gyroStep;
+ private final Servo stick;
+ private final int pilliarDist;
+ private final boolean red;
+ private final boolean rear;
+ private int[] encoderCache = new int[4];
+ private int[] startEncoderCache = new int[4];
+ private int pilliarCount;
+ private boolean stickPulled = false;
+ private FilterLib.MovingWindowFilter movingAvg;
+ private final double stickDown;
+ private final double stickUp;
+
+ private double lastTime = 0;
+ private int foundCount = 0;
+
+ private static final int APDS_FOUND_COUNT = 4;
+ private static final int COUNTS_BETWEEN_PILLIAR = 250;
+ private static final int COUNT_OH_SHIT_REAR = COUNTS_BETWEEN_PILLIAR * 7;
+ private static final int COUNT_OH_SHIT_FRONT = COUNT_OH_SHIT_REAR * 9;
+
+ APDSFind(Servo stick, double stickDown, double stickUp, APDS9960 sens, SensorLib.PID errorPid,
+ GyroCorrectStep correctIt, int dist, int error, int pilliarSkipCount, int skipDist, OpMode mode, boolean red, boolean rear) {
+ this.errorPid = errorPid;
+ this.sens = sens;
+ this.gyroStep = correctIt;
+ this.mError = error;
+ this.mDist = dist;
+ this.mode = mode;
+ this.stick = stick;
+ this.pilliarCount = pilliarSkipCount;
+ this.pilliarDist = skipDist;
+ this.stickDown = stickDown;
+ this.stickUp = stickUp;
+ this.red = red;
+ this.rear = rear;
+ }
+
+ public boolean loop() {
+ super.loop();
+ //get distance
+ double dist = this.sens.getLinearizedDistance(red);
+ if(Double.isInfinite(dist)) dist = 150;
+ if(movingAvg == null) {
+ movingAvg = new FilterLib.MovingWindowFilter(10, dist);
+ for (int i = 0; i < 4; i++) startEncoderCache[i] = gyroStep.getMotors()[i].getCurrentPosition();
+ //check stick distance when we first drop to see if we need to p[ick up immediately
+ if(pilliarCount > 0 && dist <= pilliarDist) {
+ stick.setPosition(stickUp);
+ markEncoders();
+ stickPulled = true;
+ try { TimeUnit.MILLISECONDS.sleep(250); }
+ catch (InterruptedException e) {}
+ return false;
+ }
+ }
+ movingAvg.appendValue(dist);
+ double filteredDist = movingAvg.currentValue();
+ //stupid check
+ //if any of the encoders are past the OH SHIT threshold, abort
+ boolean shit = false;
+ for(int i = 0; i < 4; i++) {
+ shit |= Math.abs(gyroStep.getMotors()[i].getCurrentPosition() - startEncoderCache[i]) >= (rear ? COUNT_OH_SHIT_REAR : COUNT_OH_SHIT_FRONT);
+ mode.telemetry.addData("Motor " + i, Math.abs(gyroStep.getMotors()[i].getCurrentPosition() - startEncoderCache[i]));
+ }
+ if(shit) {
+ setMotorsWithoutGyro(0);
+ throw new IllegalStateException("Ur bumfucked m8");
+ }
+ //if we aren't skipping any more pilliars
+ if(pilliarCount == 0) {
+ if(lastTime == 0) lastTime = mode.getRuntime() - 1;
+ //get the distance and error
+ float error = (float)(this.mDist - dist);
+ //if we found it, stop
+ //if the peak is within stopping margin, stop
+ if(Math.abs(error) <= mError) {
+ setMotorsWithoutGyro(0);
+ return ++foundCount >= APDS_FOUND_COUNT;
+ }
+ else foundCount = 0;
+ //PID
+ double time = mode.getRuntime();
+ float pError = errorPid.loop((float)(this.mDist - filteredDist), (float)(time - lastTime));
+ lastTime = time;
+ mode.telemetry.addData("power error", pError);
+ //cut out a middle range, but handle positive and negative
+ float power;
+ if(red) pError = -pError;
+ if(pError >= 0) power = Range.clip(gyroStep.getMinPower() + pError, gyroStep.getMinPower(), gyroStep.getMaxPower());
+ else power = Range.clip(pError - gyroStep.getMinPower(), -gyroStep.getMaxPower(), -gyroStep.getMinPower());
+ //if(pError >= 0) power = Range.clip(pError, 0, gyroStep.getMaxPower());
+ //else power = Range.clip(pError, -gyroStep.getMaxPower(), 0);
+ gyroStep.setPower(-power);
+ gyroStep.loop();
+ //telem
+ mode.telemetry.addData("APDS dist", error);
+ }
+ else {
+ //else skip them pilliars
+ //if stick is pulled, keep stick pulled until driving is done, then drop and decrement pilliars
+ if(stickPulled && checkEncoders(COUNTS_BETWEEN_PILLIAR)) {
+ stick.setPosition(stickDown);
+ pilliarCount--;
+ stickPulled = false;
+ }
+ //else if stick isn't pulled, but the sensor found a pilliar
+ //lift the stick and mark the encoders to drive
+ else if(!stickPulled && dist <= pilliarDist) {
+ stick.setPosition(stickUp);
+ markEncoders();
+ stickPulled = true;
+ }
+ //set motors to corrected
+ gyroStep.loop();
+ }
+ return false;
+ }
+
+ private void setMotorsWithoutGyro(float power) {
+ for(DcMotor motor : gyroStep.getMotors()) motor.setPower(power);
+ }
+
+ private boolean checkEncoders(int dist) {
+ boolean done = true;
+ DcMotor[] motorRay = gyroStep.getMotors();
+ for(int i = 0; i < 4; i++) done &= (Math.abs(motorRay[i].getCurrentPosition() - encoderCache[i]) >= dist);
+ return done;
+ }
+
+ private void markEncoders() {
+ DcMotor[] motorRay = gyroStep.getMotors();
+ for (int i = 0; i < 4; i++) encoderCache[i] = motorRay[i].getCurrentPosition();
+ }
+ }
+
+ public static class APDSBallFind extends AutoLib.Step {
+ private final boolean red;
+ private BallColor color;
+ private final AutoLib.Step whackLeft;
+ private final AutoLib.Step whackRight;
+
+ APDSBallFind(boolean red, BallColor color, AutoLib.Step whackLeft, AutoLib.Step whackRight) {
+ this.red = red;
+ this.color = color;
+ this.whackLeft = whackLeft;
+ this.whackRight = whackRight;
+ }
+
+ public boolean loop() {
+ super.loop();
+ //run appropriete sewunce
+ if(color == BallColor.LeftBlue) {
+ if(red) return whackLeft.loop();
+ else return whackRight.loop();
+ }
+ else if(color == BallColor.LeftRed) {
+ if(red) return whackRight.loop();
+ else return whackLeft.loop();
+ }
+ //or finish immedietly
+ return true;
+ }
+ }
+
+ public static class MoveToPositionStep extends AutoLib.Step {
+ private final DcMotorEx motor;
+ private final int pos;
+ private final int tol;
+ private final float power;
+ private final boolean restoreMode;
+ private DcMotor.RunMode storeMode;
+ private DcMotor.ZeroPowerBehavior storeZero;
+ private int lastTol;
+
+ MoveToPositionStep(DcMotorEx motor, int pos, int tol, float power, boolean restoreMode) {
+ this.motor = motor;
+ this.pos = pos;
+ this.tol = tol;
+ this.power = power;
+ this.restoreMode = restoreMode;
+ }
+
+ public boolean loop() {
+ super.loop();
+
+ if(firstLoopCall()) {
+ storeMode = motor.getMode();
+ if(storeMode == DcMotor.RunMode.RUN_TO_POSITION) lastTol = motor.getTargetPositionTolerance();
+ storeZero = motor.getZeroPowerBehavior();
+ motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ motor.setTargetPositionTolerance(tol);
+ motor.setTargetPosition(pos);
+ motor.setPower(power);
+ }
+
+ if(!motor.isBusy()) {
+ motor.setPower(0);
+ if(restoreMode) {
+ motor.setZeroPowerBehavior(storeZero);
+ motor.setMode(storeMode);
+ if(storeMode == DcMotor.RunMode.RUN_TO_POSITION) motor.setTargetPositionTolerance(lastTol);
+ }
+ return true;
+ }
+ return false;
+ }
+
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/LiftFix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/LiftFix.java
new file mode 100644
index 00000000000..93564747100
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/LiftFix.java
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2018;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 2/23/2018.
+ */
+
+@TeleOp(name="Motor Fixer")
+//bwahahaha were going to worlds
+public class LiftFix extends OpMode {
+ BotHardware bot = new BotHardware(this);
+
+ public void init() {
+ bot.init();
+ DcMotor[] ray = bot.getMotorRay();
+ for(DcMotor motor : ray)
+ motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ public void start() {
+
+ }
+
+ public void loop() {
+ BotHardware.Motor.relic.motor.setPower(gamepad1.left_stick_y / 2.0);
+ BotHardware.Motor.lift.motor.setPower(gamepad1.right_stick_y / 2.0);
+
+ telemetry.addData("Relic", BotHardware.Motor.relic.motor.getCurrentPosition());
+ telemetry.addData("Lift", BotHardware.Motor.lift.motor.getCurrentPosition());
+ }
+
+ public void stop() {
+ bot.stopAll();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/RatBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/RatBot.java
new file mode 100644
index 00000000000..bd90fcf7ddb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/RatBot.java
@@ -0,0 +1,95 @@
+/*
+ Basic rat bot program for testing
+ */
+
+package org.firstinspires.ftc.teamcode.opmodes.old2018;
+
+import android.app.Activity;
+import android.graphics.Color;
+import android.view.ViewGroup;
+import android.widget.RelativeLayout;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.PIDCoefficients;
+import com.qualcomm.robotcore.hardware.UltrasonicSensor;
+
+import org.firstinspires.ftc.robotcontroller.external.samples.SensorREVColorDistance;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+import java.util.LinkedList;
+
+@TeleOp(name = "RatBot", group = "Misc.")
+@Disabled
+public class RatBot extends OpMode {
+
+ private static final int DATA_MAX = 100;
+
+ DcMotorEx frontLeftMotor;
+ DcMotorEx backLeftMotor;
+ DcMotorEx frontRightMotor;
+ DcMotorEx backRightMotor;
+
+ ColorSensor mahColor;
+ DistanceSensor mahDistance;
+ DigitalChannel touch;
+
+ UltrasonicSensor side;
+ UltrasonicSensor front;
+
+ @Override
+ public void init() {
+
+ telemetry.addData("Status", "Initialized");
+
+ // hardware maps frontLeftMotor = (DcMotorEx) hardwareMap.dcMotor.get("front_left");
+ frontRightMotor = (DcMotorEx) hardwareMap.dcMotor.get("front_right");
+ backLeftMotor = (DcMotorEx) hardwareMap.dcMotor.get("back_left");
+ backRightMotor = (DcMotorEx) hardwareMap.dcMotor.get("back_right");
+
+ mahColor = hardwareMap.get(ColorSensor.class, "color");
+ mahDistance = hardwareMap.get(DistanceSensor.class, "color");
+ touch = hardwareMap.get(DigitalChannel.class, "touch");
+ touch.setMode(DigitalChannel.Mode.INPUT);
+
+ side = hardwareMap.get(UltrasonicSensor.class, "ul_side");
+ front = hardwareMap.get(UltrasonicSensor.class, "ul_front");
+
+ // change directions if necessary
+ frontLeftMotor.setDirection(DcMotor.Direction.FORWARD);
+ frontRightMotor.setDirection(DcMotor.Direction.REVERSE);
+ backLeftMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+ backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ }
+
+ @Override
+ public void start() {}
+
+ @Override
+ public void loop() {
+ // run the drive train motors
+ frontLeftMotor.setPower(gamepad1.left_stick_y);
+ frontRightMotor.setPower(gamepad1.right_stick_y);
+ backLeftMotor.setPower(gamepad1.left_stick_y);
+ backRightMotor.setPower(gamepad1.right_stick_y);
+
+ telemetry.addData("color", mahColor.red());
+ telemetry.addData("color distance", mahDistance.getDistance(DistanceUnit.MM));
+
+ telemetry.addData("touch", touch.getState());
+
+ telemetry.addData("side status", side.status());
+ telemetry.addData("front status", front.status());
+ telemetry.addData("side", side.getUltrasonicLevel());
+ telemetry.addData("front", front.getUltrasonicLevel());
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/SimpleDriveAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/SimpleDriveAuto.java
new file mode 100644
index 00000000000..893b022b134
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/SimpleDriveAuto.java
@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2018;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 12/2/2017.
+ */
+
+@Autonomous(name="Everything's Broken Drive", group="test")
+@Disabled
+public class SimpleDriveAuto extends OpMode {
+ private BotHardware bot = new BotHardware(this);
+ private AutoLib.Sequence mSeq = new AutoLib.LinearSequence();
+
+ public void init() {
+ bot.init();
+ mSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 270.0f, 1600, true));
+ }
+
+ @Override
+ public void start() {
+ //hmmm
+ }
+
+ @Override
+ public void loop() {
+ if(mSeq.loop()) requestOpModeStop();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/Teleop.java
new file mode 100644
index 00000000000..da821516d92
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/Teleop.java
@@ -0,0 +1,275 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2018;
+
+import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+import org.firstinspires.ftc.teamcode.libraries.hardware.StupidColor;
+import org.firstinspires.ftc.teamcode.opmodes.demo.BlinkTest;
+
+import java.util.Random;
+import java.util.concurrent.TimeUnit;
+
+/**
+ * Created by Noah on 10/27/2017.
+ * Teleop!
+ */
+
+@TeleOp(name="Teleop")
+public class Teleop extends OpMode {
+ private static final float slowFactor = 0.8f;
+ private static final float fastFactor = 1.0f;
+ private static final double SERVO_INC_SHAKE = 0.1;
+ private static final double SERVO_INC_MAX = 0.02;
+ private static final double SERVO_INC_MIN = 0.001;
+ private static final int LIFT_COUNTS = 2800; //4250
+ private static final int LIFT_BOTTOM_COUNTS = 730;
+ private static final int LIFT_INC_COUNTS = 100;
+ private static final int RELIC_ARM_COUNTS = 6450;
+ private static final double BUCKET_SHAKE_INTERVAL = 0.04; //seconds
+ private static final double BUCKET_FLAT = 0.58;
+ //private static final int BUCKET_LIFT_COUNTS = 50;
+ //private static final int INTAKE_LIFT_COUNTS = 1200;
+ private static final int MIN_VELOCITY = 550;
+ private static final float ARM_GRAB = 0.39f;
+
+ private static final double ARM_M_COFF = 0.0000472907349606521;
+ private static final double ARM_B_COFF = 0.0276268627304199;
+ private static final int ARM_MIN_COUNTS = 3000;
+
+ private static final int RELIC_AUTO_ARM_COUNTS = 6200;
+
+ protected BotHardware bot = new BotHardware(this);
+ private boolean lastA = false;
+ private boolean robotSlow = false;
+ private boolean motorsSet = false;
+ private boolean lastY = false;
+ private boolean grabOpen = true;
+
+ private boolean suckMotorStopped = false;
+ private boolean suckLeftStopped = false;
+ private boolean suckMotorRamping = false;
+ private int suckMotorRampCount = 0;
+
+ private int liftPos;
+ private int relicPos;
+
+ private boolean servoSet = false;
+ private double lastTime = 0;
+
+ private boolean xLock = false;
+
+ public void init() {
+ bot.init();
+ DcMotor[] ray = bot.getMotorRay();
+ for(DcMotor motor : ray){
+ motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ }
+
+ bot.setLights(0.5);
+ }
+
+ public void start() {
+ gamepad1.setJoystickDeadzone(0.05f);
+ gamepad2.setJoystickDeadzone(0.05f);
+ bot.start();
+ bot.setDropPos(BotHardware.ServoE.backDropUp);
+ BotHardware.Motor.lift.motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ BotHardware.Motor.relic.motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ BotHardware.Motor.relic.motor.setTargetPositionTolerance(5);
+ BotHardware.Motor.relic.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ liftPos = BotHardware.Motor.lift.motor.getCurrentPosition();
+ relicPos = BotHardware.Motor.relic.motor.getCurrentPosition();
+ BotHardware.Motor.suckRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ BotHardware.Motor.suckLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ }
+
+ public void loop() {
+ //calc deltas
+ final int liftPosDelta = BotHardware.Motor.lift.motor.getCurrentPosition() - liftPos;
+
+ if(gamepad1.a && !lastA) robotSlow = !robotSlow;
+ lastA = gamepad1.a;
+
+ if(gamepad1.b || gamepad2.b) bot.setDropPos(BUCKET_FLAT);
+ if(gamepad2.a) bot.setDropPos(BotHardware.ServoE.backDropUp);
+
+ //lowering
+ if(gamepad2.left_bumper) {
+ bot.setLiftMotors(-1.0f);
+ BotHardware.Motor.lift.motor.setTargetPosition(liftPos);
+ }
+ else if(gamepad2.right_bumper) {
+ bot.setLiftMotors(1.0f);
+ BotHardware.Motor.lift.motor.setTargetPosition(liftPos + LIFT_COUNTS);
+ }
+ else bot.setLiftMotors(0);
+
+ if(gamepad1.left_trigger > 0)
+ bot.setDropPos(Range.clip(bot.getDropPos() + Range.scale(gamepad1.left_trigger, 0, 1, SERVO_INC_MIN, SERVO_INC_MAX), BotHardware.ServoE.backDropDown, BotHardware.ServoE.backDropUp));
+ else if(gamepad1.right_trigger > 0) {
+ if(liftPosDelta > LIFT_BOTTOM_COUNTS) bot.setDropPos(Range.clip(bot.getDropPos() - Range.scale(gamepad1.right_trigger, 0, 1, SERVO_INC_MIN, SERVO_INC_MAX), BotHardware.ServoE.backDropDown, BotHardware.ServoE.backDropUp));
+ else {
+ BotHardware.Motor.lift.motor.setTargetPosition(liftPos + LIFT_BOTTOM_COUNTS + 50);
+ bot.setLiftMotors(1.0f);
+ }
+ }
+
+ double time = getRuntime();
+ if(gamepad1.x && time - lastTime >= BUCKET_SHAKE_INTERVAL) {
+ telemetry.addData("Interval", time - lastTime);
+ lastTime = time;
+ if(servoSet) bot.setDropPos(Range.clip(bot.getDropPos() - SERVO_INC_SHAKE, BotHardware.ServoE.backDropDown, BotHardware.ServoE.backDropUp));
+ else bot.setDropPos(Range.clip(bot.getDropPos() + SERVO_INC_SHAKE, BotHardware.ServoE.backDropDown, BotHardware.ServoE.backDropUp));
+ servoSet = !servoSet;
+ }
+ else if(!gamepad1.x) servoSet = false;
+
+ telemetry.addData("Drop", BotHardware.ServoE.backDropLeft.servo.getPosition());
+
+
+ if(gamepad2.start) {
+ /*
+ //relic automation!
+ telemetry.addData("Relic Automation!", true);
+ xLock = false;
+ //put arm to farthest point
+ BotHardware.Motor.relic.motor.setTargetPosition(relicPos + RELIC_ARM_COUNTS);
+ BotHardware.Motor.relic.motor.setPower(1.0f);
+ //if over first counts, drop arm servo to place
+ final int pos = Math.abs(BotHardware.Motor.relic.motor.getCurrentPosition() - relicPos);
+ if(pos >= RELIC_AUTO_ARM_COUNTS) BotHardware.ServoE.arm.servo.setPosition(pos * ARM_M_COFF + ARM_B_COFF);
+ if(pos > RELIC_ARM_COUNTS - 15) {
+ //open grab
+ BotHardware.ServoE.grab.servo.setPosition(BotHardware.ServoE.grabOpen);
+ grabOpen = true;
+ }
+ */
+ xLock = false;
+ BotHardware.ServoE.arm.servo.setPosition(0.39);
+ }
+ else {
+ //driver control
+ if(gamepad2.dpad_up) {
+ BotHardware.ServoE.arm.servo.setPosition(Range.clip(BotHardware.ServoE.arm.servo.getPosition() + 0.01, BotHardware.ServoE.armClosed, BotHardware.ServoE.armOpen));
+ xLock = false;
+ }
+ else if(gamepad2.dpad_down){
+ BotHardware.ServoE.arm.servo.setPosition(Range.clip(BotHardware.ServoE.arm.servo.getPosition() - 0.01, BotHardware.ServoE.armClosed, BotHardware.ServoE.armOpen));
+ xLock = false;
+ }
+
+ if(xLock || gamepad2.x){
+ final int relPos = Math.abs(BotHardware.Motor.relic.motor.getCurrentPosition() - relicPos);
+ final double pos = relPos * ARM_M_COFF + ARM_B_COFF;
+ if(relPos > ARM_MIN_COUNTS) BotHardware.ServoE.arm.servo.setPosition(pos);
+ else BotHardware.ServoE.arm.servo.setPosition(0.2);
+ //BotHardware.ServoE.arm.servo.setPosition(ARM_GRAB);
+ xLock = true;
+ }
+
+ telemetry.addData("XLock", xLock);
+
+ if(gamepad2.y && !lastY) {
+ if(grabOpen = !grabOpen) BotHardware.ServoE.grab.servo.setPosition(BotHardware.ServoE.grabOpen);
+ else BotHardware.ServoE.grab.servo.setPosition(BotHardware.ServoE.grabClosed);
+ }
+ lastY = gamepad2.y;
+
+ if(gamepad2.left_trigger > 0) {
+ BotHardware.Motor.relic.motor.setTargetPosition(relicPos + 250);
+ BotHardware.Motor.relic.motor.setPower(-gamepad2.left_trigger);
+ }
+ else if(gamepad2.right_trigger > 0){
+ BotHardware.Motor.relic.motor.setTargetPosition(relicPos + RELIC_ARM_COUNTS);
+ BotHardware.Motor.relic.motor.setPower(gamepad2.right_trigger);
+ }
+ else BotHardware.Motor.relic.motor.setPower(0);
+ }
+
+ if(gamepad2.left_stick_y <= 0.95 || gamepad2.right_stick_y <= 0.95) {
+ bot.setSuckLeft(gamepad2.left_stick_y);
+ bot.setSuckRight(gamepad2.right_stick_y);
+ suckMotorRamping = true;
+ suckMotorStopped = false;
+ suckMotorRampCount = 5;
+ }
+ else {
+ telemetry.addData("Metering", true);
+
+ double lastRight = BotHardware.Motor.suckRight.motor.getVelocity(AngleUnit.DEGREES);
+ double lastLeft = BotHardware.Motor.suckLeft.motor.getVelocity(AngleUnit.DEGREES);
+
+ if(suckMotorRamping) {
+ BotHardware.Motor.suckLeft.motor.setPower(1);
+ BotHardware.Motor.suckRight.motor.setPower(1);
+ if(lastRight > MIN_VELOCITY && lastLeft > MIN_VELOCITY) suckMotorRampCount++;
+ else suckMotorRampCount = 0;
+ if(suckMotorRampCount >= 10) {
+ suckMotorRamping = false;
+ suckMotorRampCount = 0;
+ }
+ }
+ else if(!suckMotorStopped && (lastRight < MIN_VELOCITY || lastLeft < MIN_VELOCITY)) {
+ if(lastLeft < lastRight) {
+ BotHardware.Motor.suckRight.motor.setPower(0);
+ suckLeftStopped = false;
+ }
+ else {
+ BotHardware.Motor.suckLeft.motor.setPower(0);
+ suckLeftStopped = true;
+ }
+ suckMotorStopped = true;
+ }
+ else if(suckMotorStopped && ((suckLeftStopped && lastRight > MIN_VELOCITY) || (!suckLeftStopped && lastLeft > MIN_VELOCITY))) {
+ suckMotorStopped = false;
+ suckMotorRamping = true;
+ }
+ }
+
+ //if(dropperDown) bot.dropBack();
+ //else bot.raiseBack();
+
+ if(gamepad1.y) {
+ bot.setLeftDrive(gamepad1.left_stick_y);
+ bot.setRightDrive(gamepad1.right_stick_y);
+ }
+ else if(robotSlow) {
+ if(!motorsSet) {
+ DcMotor[] ray = bot.getMotorRay();
+ for(DcMotor motor : ray) motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ motorsSet = true;
+ }
+ bot.setLeftDrive(gamepad1.left_stick_y * slowFactor);
+ bot.setRightDrive(gamepad1.right_stick_y * slowFactor);
+ }
+ else {
+ bot.setLeftDrive(gamepad1.left_stick_y * fastFactor);
+ bot.setRightDrive(gamepad1.right_stick_y * fastFactor);
+ if(motorsSet) {
+ DcMotor[] ray = bot.getMotorRay();
+ for(DcMotor motor : ray) motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+ motorsSet = false;
+ }
+ }
+
+
+ bot.getStickBase().setPosition(BotHardware.ServoE.stickBaseHidden);
+
+ telemetry.addData("Back Drop", bot.getDropPos());
+ telemetry.addData("Robot Slow", robotSlow);
+ telemetry.addData("Lift", liftPos);
+ telemetry.addData("Arm", BotHardware.ServoE.arm.servo.getPosition());
+ telemetry.addData("Relic Delta", Math.abs(BotHardware.Motor.relic.motor.getCurrentPosition() - relicPos));
+ }
+
+ public void stop() {
+ bot.stopAll();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/UltraAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/UltraAuto.java
new file mode 100644
index 00000000000..3dbdfdf8b04
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017-18/UltraAuto.java
@@ -0,0 +1,401 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2018;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
+import com.qualcomm.robotcore.util.Range;
+import com.vuforia.CameraDevice;
+import com.vuforia.Vec2F;
+
+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.RelicRecoveryVuMark;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.EncoderHoneStep;
+import org.firstinspires.ftc.teamcode.libraries.GyroCorrectStep;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.VuforiaBallLib;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+import org.firstinspires.ftc.teamcode.libraries.hardware.MatbotixUltra;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.HeadingSensor;
+import org.opencv.core.Mat;
+
+import java.util.LinkedList;
+import java.util.List;
+
+/**
+ * Created by Noah on 3/25/2018.
+ * Attempt at moving the cryptopox position sensor to ultrasonic for speed
+ * This opmode will work as fast as possible to get more points with multiple glyphs
+ */
+
+@Autonomous(name="Ultra Auto")
+@Disabled
+public class UltraAuto extends VuforiaBallLib {
+ //CONSTANTS
+ private static final float PILLAR_SPACING_INCH = 7.63f;
+ private static final float ENCODE_PER_CM = 17.1f;
+ private static final float DRIVE_DUMP_CM = 16.5f;
+ private static final float ONE_SIDE_PUSH_TIME = 0.25f;
+ private static final int[] RED_ZERO = new int[] {33 /* the offset from the ultrasonic sensor to the wall when the robot is centered on the balance pad */,
+ 104 /* the ofsett from the back wall when front of robot is aligned with mat edge off balance pad */};
+ private static final int[] BLUE_ZERO = new int[] {20, 80};
+ private static final int Y_STUPID_MAX = 120;
+ private static final int Y_STUPID_MIN = 90;
+ private static final int Y_STUPID_COUNT = 4;
+ private static final int X_STUPID_MAX = 60;
+ private static final float X_ANGLE_MAX = 3.0f;
+ private static final double BALL_WAIT_SEC = 2.0;
+
+ private final SensorLib.PID encoderHonePID = new SensorLib.PID(3.88f, 9.00f, 0, 50);
+ private final SensorLib.PID gyroDrivePID = new SensorLib.PID(-64.0f, 0, 0, 0);
+ private final SensorLib.PID gyroTurnPID = new SensorLib.PID(0.0056f, 0.0063f, 0, 7);
+ private final SensorLib.PID gyroTurnPIDFast = new SensorLib.PID(0.007f, 0.0063f, 0, 7);
+
+ private enum AutoPath {
+ RED_FRONT_RIGHT(true, false, RelicRecoveryVuMark.RIGHT, (float)DistanceUnit.INCH.toCm(0), 125),
+ RED_FRONT_CENTER(true, false, RelicRecoveryVuMark.CENTER, (float)DistanceUnit.INCH.toCm(PILLAR_SPACING_INCH), 125),
+ RED_FRONT_LEFT(true, false, RelicRecoveryVuMark.LEFT, (float)DistanceUnit.INCH.toCm(2), 55);
+
+ public final RelicRecoveryVuMark vuMark;
+ public final boolean red;
+ public final boolean rear;
+ public final float distanceCM;
+ public final float turnAmount;
+ AutoPath(boolean red, boolean rear, RelicRecoveryVuMark mark, float travelCM, float turnAmount) {
+ this.red = red;
+ this.rear = rear;
+ this.vuMark = mark;
+ this.distanceCM = travelCM;
+ this.turnAmount = turnAmount;
+ }
+ }
+
+ private enum State {
+ JEWEL,
+ MEASURE,
+ BLOCK,
+ MULTIBLOCK
+ }
+
+ //ADJUSTABLE VARIBLES
+ protected boolean red = true;
+ protected boolean rear = false;
+
+ //DEVICES
+ private final BotHardware bot = new BotHardware(this);
+
+ private enum UltraPos {
+ FRONT("ultrafront"),
+ BACK("ultraback"),
+ LEFT("ultraleft"),
+ RIGHT("ultraright");
+
+ public final String name;
+ public MatbotixUltra sensor;
+
+ UltraPos(String name) {
+ this.name = name;
+ }
+
+ public static void init(OpMode mode) {
+ for(UltraPos pos : values()) {
+ pos.sensor = new MatbotixUltra(mode.hardwareMap.get(I2cDeviceSynch.class, pos.name));
+ pos.sensor.initDevice();
+ pos.sensor.startDevice();
+ }
+ }
+
+ public static MatbotixUltra getXSensor() {
+ return RIGHT.sensor;
+ }
+
+ public static MatbotixUltra getYSensor(boolean red, boolean rear) {
+ if(rear || red) return BACK.sensor;
+ return FRONT.sensor;
+ }
+ }
+
+ //MEMBERS
+ private AutoLib.LinearSequence mSeq = new AutoLib.LinearSequence();
+ private boolean firstRun = false;
+ private int xOffsetStart;
+ private int yOffsetStart;
+ private double startLoop = 0;
+ private BallColor color = BallColor.Undefined;
+ private BallColor altColor = BallColor.Undefined;
+ private State state = State.JEWEL;
+
+ //CODE
+ public void init() {
+ bot.init();
+ bot.start();
+ UltraPos.init(this);
+ bot.setDropPos(0.62);
+ //TODO: Ball stuff
+ initVuforia(true);
+ startTracking();
+ }
+
+ public void init_loop() {
+ for(UltraPos ultra : UltraPos.values()) telemetry.addData(ultra.name, ultra.sensor.getReading());
+ telemetry.addData("Ball Color", getBallColor().toString());
+ telemetry.addData("IMU", bot.getHeadingSensor().getHeading());
+ }
+
+ public void start() {
+
+ }
+
+ public void loop() {
+ //detect jewels
+ if(state == State.JEWEL) {
+ if(startLoop == 0) startLoop = getRuntime();
+ if(getRuntime() - startLoop >= BALL_WAIT_SEC / 2 && !firstRun) CameraDevice.getInstance().setFlashTorchMode(true);
+ if(getRuntime() - startLoop < BALL_WAIT_SEC && (color == BallColor.Indeterminate || color == BallColor.Undefined)) {
+ color = getBallColor();
+ altColor = getCVBallColor();
+ }
+ else state = State.MEASURE;
+ }
+ //drive and measure distances from walls
+ if(state == State.MEASURE) {
+ if(!firstRun) {
+ //initialize starting sequence
+ CameraDevice.getInstance().setFlashTorchMode(false);
+ //init whacky stick code here
+ if(color == BallColor.Indeterminate || color == BallColor.Undefined) color = altColor;
+ if(color == BallColor.LeftBlue || color == BallColor.LeftRed) {
+ final AutoLib.Sequence whack = new AutoLib.LinearSequence();
+ if(red) whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed, 0.25, false));
+ else whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue, 0.25, false));
+ whack.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickDown, 0.5, false));
+ mSeq.add(whack);
+
+ //hmmmmm
+ final AutoLib.Step whackLeft;
+ if(red) whackLeft = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed - BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ else whackLeft = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue - BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ final AutoLib.Step whackRight;
+ if(red) whackRight = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed + BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ else whackRight = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue + BotHardware.ServoE.stickBaseSwingSize, 0.5, false);
+ if(color == BallColor.LeftBlue) {
+ if(red) mSeq.add(whackLeft);
+ else mSeq.add(whackRight);
+ }
+ else {
+ if(red) mSeq.add(whackRight);
+ else mSeq.add(whackLeft);
+ }
+
+ final AutoLib.Sequence whackUp = new AutoLib.LinearSequence();
+ whackUp.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickUp, 0.25, false));
+ whackUp.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseHidden, 0, false));
+ mSeq.add(whackUp);
+ }
+ //TODO: Auto path
+ //drive and measure
+ mSeq.add(new AutoLib.AzimuthCountedDriveStep(this, 0, bot.getHeadingSensor(), gyroDrivePID, bot.getMotorVelocityShimArray(),
+ -550, 500, false, -720, -55));
+ mSeq.add(new DriveAndMeasureStep(this, 0, bot.getHeadingSensor(), gyroDrivePID, bot.getMotorVelocityShimArray(),
+ -550, 200, true, -720, -55));
+ mSeq.add(new WaitMotorVelocityStep(new DcMotorEx[] {BotHardware.Motor.frontLeft.motor, BotHardware.Motor.frontRight.motor}, 10));
+ firstRun = true;
+ }
+ if(mSeq.loop()) {
+ //measure y
+ final int yZero = red ? RED_ZERO[1] : BLUE_ZERO[1];
+ int reading = UltraPos.getYSensor(red, rear).getReading();
+ int count = 0;
+ while ((reading >= Y_STUPID_MAX || reading <= Y_STUPID_MIN) && ++count < Y_STUPID_COUNT) reading = UltraPos.getYSensor(red, rear).getReading();
+ if(count >= Y_STUPID_COUNT); //TODO: Failure
+ yOffsetStart = reading - yZero;
+ //switch state
+ state = State.BLOCK;
+ firstRun = false;
+ }
+ }
+ //execute based on above values
+ if(state == State.BLOCK) {
+ if(!firstRun) {
+ //initialize sequence with found values
+ AutoPath path = AutoPath.RED_FRONT_RIGHT;
+
+ mSeq = new AutoLib.LinearSequence();
+
+ final AutoLib.GyroTurnStep turn = new AutoLib.GyroTurnStep(this, path.turnAmount, bot.getHeadingSensor(), bot.getMotorRay(), 0.04f, 0.7f, gyroTurnPID, 2f, 5, true);
+ mSeq.add(makeDriveToBoxSeq(this, bot, encoderHonePID,
+ makeGyroDriveStep(0, gyroDrivePID, 100, 55, 720),
+ turn,
+ makeGyroDriveStep(path.turnAmount, gyroDrivePID, 500, 55, 720),
+ new DcMotor[] {BotHardware.Motor.frontLeft.motor, BotHardware.Motor.frontRight.motor},
+ /*Math.round(path.distanceCM * ENCODE_PER_CM)*/0, Math.round(DRIVE_DUMP_CM * ENCODE_PER_CM),
+ 5, 10, xOffsetStart, yOffsetStart));
+ mSeq.add(bot.getDropStep());
+ final AutoLib.ConcurrentSequence backAndDown = new AutoLib.ConcurrentSequence();
+ backAndDown.add(bot.getReverseDropStep());
+ backAndDown.add(bot.getLiftLowerStep());
+ AutoLib.ConcurrentSequence oneSideSeq = new AutoLib.ConcurrentSequence();
+ DcMotor[] temp = bot.getMotorRay();
+ if(path.turnAmount > 55) {
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[0], 0.7f, ONE_SIDE_PUSH_TIME, true));
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[1], 0.7f, ONE_SIDE_PUSH_TIME, true));
+ }
+ else {
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[2], 0.7f, ONE_SIDE_PUSH_TIME, true));
+ oneSideSeq.add(new AutoLib.TimedMotorStep(temp[3], 0.7f, ONE_SIDE_PUSH_TIME, true));
+ }
+ backAndDown.add(oneSideSeq);
+ mSeq.add(backAndDown);
+
+ mSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), -720, 200, true));
+ mSeq.add(new AutoLib.GyroTurnStep(this, red ? 90 : -90, bot.getHeadingSensor(), bot.getMotorRay(), 0.04f, 0.7f, gyroTurnPID, 3f, 5, true));
+
+ firstRun = true;
+ }
+ if(mSeq.loop()) {
+ //multiblock bby
+ state = State.MULTIBLOCK;
+ firstRun = false;
+ }
+ }
+ if(state == State.MULTIBLOCK) {
+ //TODO: this
+ if(!firstRun) {
+
+ firstRun = true;
+ }
+ if(mSeq.loop()) {
+ requestOpModeStop();
+ }
+ }
+ telemetry.addData("X Offset", xOffsetStart);
+ telemetry.addData("Y Offset", yOffsetStart);
+ telemetry.addData("State", state.name());
+ }
+
+ public void stop() {
+ bot.stopAll();
+ }
+
+ private GyroCorrectStep makeGyroDriveStep(float heading, SensorLib.PID pid, float power, float powerMin, float powerMax) {
+ return new GyroCorrectStep(this, heading, bot.getHeadingSensor(), pid, bot.getMotorVelocityShimArray(), power, powerMin, powerMax);
+ }
+
+ private static AutoLib.Sequence makeDriveToBoxSeq(OpMode mode, BotHardware bot,
+ SensorLib.PID errorPid,
+ GyroCorrectStep firstDriveStep,
+ AutoLib.GyroTurnStep turnStep,
+ GyroCorrectStep secondDriveStep,
+ DcMotor[] encoderMotors, int firstLegCounts, int secondLegCounts, int error, int count, int xOffsett, int yOffsett) {
+ //step zero: calculate how far the robot must drive according to our x and y offset
+ //subtract the extra distance needed to be travelled in the second leg from the first leg
+ final double rad = Math.toRadians(90 - turnStep.getTargetHeading());
+ firstLegCounts += Math.round(xOffsett * Math.tan(rad) * ENCODE_PER_CM);
+ firstLegCounts -= Math.round(yOffsett * ENCODE_PER_CM);
+ //calculate the 45degree leg delta
+ secondLegCounts += Math.round(xOffsett / Math.cos(rad) * ENCODE_PER_CM);
+ //construct sequence
+ final AutoLib.LinearSequence mSeq = new AutoLib.LinearSequence();
+ //drive firstlegcounts
+ mSeq.add(new EncoderHoneStep(mode, firstLegCounts, error, count, errorPid, firstDriveStep, encoderMotors));
+ //turn
+ mSeq.add(turnStep);
+ //drive secondlegcounts
+ AutoLib.ConcurrentSequence liftAndDrive = new AutoLib.ConcurrentSequence();
+ liftAndDrive.add(bot.getLiftRaiseStep());
+ //liftAndDrive.add(new EncoderHoneStep(mode, -secondLegCounts, error, count, errorPid, secondDriveStep, encoderMotors));
+ liftAndDrive.add(new CheapEncoderGyroStep(secondDriveStep, encoderMotors, secondLegCounts - 30));
+ mSeq.add(liftAndDrive);
+ return mSeq;
+ }
+
+ private static class WaitMotorVelocityStep extends AutoLib.Step {
+ private final DcMotorEx[] motors;
+ private final float velThresh;
+
+ WaitMotorVelocityStep(DcMotorEx[] motors, float velThresh) {
+ this.motors = motors;
+ this.velThresh = velThresh;
+ }
+
+ WaitMotorVelocityStep(DcMotorEx motor, float velThresh) {
+ this.motors = new DcMotorEx[] {motor};
+ this.velThresh = velThresh;
+ }
+
+ public boolean loop() {
+ super.loop();
+ boolean done = true;
+ for(DcMotorEx motor : motors) done &= Math.abs(motor.getVelocity(AngleUnit.DEGREES)) <= velThresh;
+ return done;
+ }
+ }
+
+ private static class CheapEncoderGyroStep extends AutoLib.Step {
+ private final GyroCorrectStep gyroStep;
+ private final int count;
+ private final DcMotor[] encoders;
+ private int[] startPos;
+
+ CheapEncoderGyroStep(GyroCorrectStep step, DcMotor[] encoders, int counts) {
+ this.gyroStep = step;
+ this.encoders = encoders;
+ this.count = counts;
+ }
+
+ public boolean loop() {
+ super.loop();
+ if(firstLoopCall()) {
+ gyroStep.reset();
+ startPos = new int[encoders.length];
+ for (int i = 0; i < encoders.length; i++) startPos[i] = encoders[i].getCurrentPosition();
+ }
+ //get the distance delta
+ int total = 0;
+ for (int i = 0; i < encoders.length; i++) {
+ total += encoders[i].getCurrentPosition() - startPos[i];
+ //mode.telemetry.addData("Delta " + i, encode[i].getCurrentPosition() - startPos[i]);
+ }
+ final float read = count - Math.abs((float)total / encoders.length);
+ if(read > 0) gyroStep.loop();
+ else {
+ for(DcMotor motor : gyroStep.getMotors()) motor.setPower(0);
+ return true;
+ }
+ return false;
+ }
+
+ }
+
+ private class DriveAndMeasureStep extends AutoLib.AzimuthCountedDriveStep {
+
+ private List data = new LinkedList<>();
+
+ DriveAndMeasureStep(OpMode mode, float heading, HeadingSensor gyro, SensorLib.PID pid,
+ DcMotor motors[], float power, int count, boolean stop, float powerMin, float powerMax) {
+ super(mode, heading, gyro, pid, motors, power, count, stop, powerMin, powerMax);
+ }
+
+ @Override
+ public boolean loop() {
+ final int reading;
+ if(super.loop()) {
+ int max = 0;
+ for(Integer i : data) if(i < X_STUPID_MAX && i > max) max = i;
+ final int xZero = red ? RED_ZERO[0] : BLUE_ZERO[0];
+ xOffsetStart = max - xZero;
+ return true;
+ }
+ else if(Math.abs(bot.getImu().getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).secondAngle) < X_ANGLE_MAX
+ && (reading = UltraPos.getXSensor().getReadingNoDelay()) >= 0)
+ data.add(reading);
+ return false;
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/AutoMotorTest1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/AutoMotorTest1.java
new file mode 100644
index 00000000000..5cc166acf3f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/AutoMotorTest1.java
@@ -0,0 +1,110 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+/**
+ * OpMode to test AutoLib driving of motors by time or encoder counts
+ * Created by phanau on 12/14/15.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+
+
+/**
+ * A test example of autonomous opmode programming using AutoLib classes.
+ * Created by phanau on 12/14/15.
+ */
+
+
+
+@Autonomous(name="Test: AutoLib Motor Step Test 1", group ="Test")
+@Disabled
+public class AutoMotorTest1 extends OpMode {
+
+ AutoLib.Sequence mSequence; // the root of the sequence tree
+ boolean bDone; // true when the programmed sequence is done
+ boolean bFirst; // true first time loop() is called
+
+ DcMotor mFr, mBr, mFl, mBl; // four drive motors (front right, back right, front left, back left)
+ DcMotor mIo, mUd; // two arm motors (in-out, up-down) OPTIONAL
+
+ boolean debug = true; // run in test/debug mode with dummy motors and data logging
+ boolean haveEncoders = false; // robot has Encoder-based motors
+
+ public AutoMotorTest1() {
+ }
+
+ public void init() {
+
+ AutoLib.HardwareFactory mf = null;
+ if (debug)
+ mf = new AutoLib.TestHardwareFactory(this);
+ else
+ mf = new AutoLib.RealHardwareFactory(this);
+
+ // get the motors: depending on the factory we created above, these may be
+ // either dummy motors that just log data or real ones that drive the hardware
+ mFr = mf.getDcMotor("fr");
+ mFl = mf.getDcMotor("fl");
+ mBr = mf.getDcMotor("br");
+ mBl = mf.getDcMotor("bl");
+
+ // OPTIONAL arm motors
+ try {
+ mIo = mf.getDcMotor("io");
+ mUd = mf.getDcMotor("ud");
+ }
+ catch (IllegalArgumentException iax) {
+ mIo = mUd = null;
+ }
+
+ mFl.setDirection(DcMotor.Direction.REVERSE);
+ mBl.setDirection(DcMotor.Direction.REVERSE);
+
+ // create the root Sequence for this autonomous OpMode
+ mSequence = new AutoLib.LinearSequence();
+
+ // add a Step (actually, a ConcurrentSequence under the covers) that
+ // drives all four motors forward at half power for 2 seconds
+ mSequence.add(new AutoLib.MoveByTimeStep(mFr, mBr, mFl, mBl, 0.5, 2.0, false));
+
+ // create a second sequence that drives motors at different speeds
+ // to turn left for 3 seconds, then stop all motors
+ mSequence.add(new AutoLib.TurnByTimeStep(mFr, mBr, mFl, mBl, 0.5, 0.2, 3.0, true));
+
+ // raise the arm using encoders while also extending it for 1 second
+ AutoLib.ConcurrentSequence cs1 = new AutoLib.ConcurrentSequence();
+ if (mUd != null && (debug || !haveEncoders))
+ cs1.add(new AutoLib.TimedMotorStep(mUd, 0.75, 1.0, true)); // we don't support encoders yet in debug mode
+ //else
+ // cs1.add(new AutoLib.EncoderMotorStep(new EncoderMotor(mUd), 0.75, 1000, true));
+ if (mIo != null)
+ cs1.add(new AutoLib.TimedMotorStep(mIo, 0.5, 1.0, true));
+ mSequence.add(cs1);
+
+ // start out not-done, first time
+ bDone = false;
+ bFirst = true;
+ }
+
+ public void loop() {
+ // reset the timer when we start looping
+ if (bFirst) {
+ this.resetStartTime(); // OpMode provides a timer
+ bFirst = false;
+ }
+
+ // until we're done with the root Sequence, perform the current Step(s) each time through the loop
+ if (!bDone) {
+ bDone = mSequence.loop(); // returns true when we're done
+
+ if (debug)
+ telemetry.addData("elapsed time", this.getRuntime());
+ }
+ }
+
+ public void stop() {
+ telemetry.addData("stop() called", "");
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAuto.java
new file mode 100644
index 00000000000..998b6e9cbf0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAuto.java
@@ -0,0 +1,91 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+/**
+ * Created by Robotics on 3/7/2017.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+
+@Autonomous(name="Block Auto (Shoot Balls, Defense)", group="Main")
+@Disabled
+public class BlockAuto extends OpMode {
+
+ BotHardwareOld robot = new BotHardwareOld();
+
+ AutoLib.Sequence mShoot;
+
+ boolean bDone = false;
+
+ boolean redColor;
+
+ OpMode modePointer;
+
+ // parameters of the PID controller for this sequence's first part
+ float Kp = 0.05f; // degree heading proportional term correction per degree of deviation
+ //float Ki = 0.02f; // ... integrator term
+ float Ki = 0.00f;
+ float Kd = 0; // ... derivative term
+ float KiCutoff = 3.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID gPid = new SensorLib.PID(Kp, Ki, Kd, KiCutoff);
+
+ public BlockAuto(OpMode mode, boolean isRed){
+ modePointer = mode;
+ redColor = isRed;
+ }
+
+ @Override
+ public void init(){
+ //init hardware objects
+ final boolean debug = false;
+ robot = new BotHardwareOld();
+ robot.init(modePointer, debug, true);
+ robot.setMaxSpeedAll(2500);
+
+ mShoot = new AutoLib.LinearSequence();
+
+ mShoot.add(new AutoLib.LogTimeStep(modePointer, "Wait", 10.5));
+ mShoot.add(new AutoLib.MoveByEncoderStep(robot.getMotorArray(), 0.4f, 1450, true));
+ mShoot.add(new AutoLib.LogTimeStep(modePointer, "YAY", 0.5));
+ mShoot.add(new AutoLib.EncoderMotorStep(robot.launcherMotor, 1.0f, 1500, true, modePointer));
+ mShoot.add(new AutoLib.TimedServoStep(robot.ballServo, 0.6f, 0.8, false));
+ mShoot.add(new AutoLib.EncoderMotorStep(robot.launcherMotor, 1.0f, 1500, true, modePointer));
+
+ int direction;
+ if(redColor) direction = 25;
+ else direction = -25;
+
+ //mShoot.add(new AutoLib.GyroTurnStep(modePointer, direction, robot.getNavXHeadingSensor(), robot.getMotorArray(), 0.4f, 3.0f, true));
+ mShoot.add(new AutoLib.MoveByEncoderStep(robot.getMotorArray(), 0.5f, 2000, true));
+ }
+
+ @Override
+ public void init_loop(){
+ modePointer.telemetry.addData("NavX Connected", robot.navX.isConnected());
+ modePointer.telemetry.addData("NavX Ready", robot.startNavX());
+ }
+
+ @Override
+ public void start(){
+ robot.navX.zeroYaw();
+ }
+
+ @Override
+ public void loop(){
+ // until we're done, keep looping through the current Step(s)
+ if (!bDone)
+ bDone = mShoot.loop(); // returns true when we're done
+ else
+ modePointer.telemetry.addData("sequence finished", "");
+ }
+
+ @Override
+ public void stop(){
+ super.stop();
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAutoBlue.java
new file mode 100644
index 00000000000..7d242129ab9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAutoBlue.java
@@ -0,0 +1,42 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+/**
+ * Created by Robotics on 3/7/2017.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Blue Block Auto (Shoot Balls, Defense)", group="Main")
+@Disabled
+public class BlockAutoBlue extends OpMode {
+ BotHardwareOld robot = new BotHardwareOld();
+
+ BlockAuto auto = new BlockAuto(this, false);
+
+ @Override
+ public void init(){
+ auto.init();
+ }
+
+ @Override
+ public void init_loop(){
+ auto.init_loop();
+ }
+
+ @Override
+ public void start(){
+ auto.start();
+ }
+
+ @Override
+ public void loop(){
+ auto.loop();
+ }
+
+ @Override
+ public void stop(){
+ auto.stop();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAutoRed.java
new file mode 100644
index 00000000000..035c7595238
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlockAutoRed.java
@@ -0,0 +1,43 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+/**
+ * Created by Robotics on 3/7/2017.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Red Block Auto (Shoot Balls, Defense)", group="Main")
+@Disabled
+public class BlockAutoRed extends OpMode {
+ BotHardwareOld robot = new BotHardwareOld();
+
+ BlockAuto auto = new BlockAuto(this, true);
+
+ @Override
+ public void init(){
+ auto.init();
+ }
+
+ @Override
+ public void init_loop(){
+ auto.init_loop();
+ }
+
+ @Override
+ public void start(){
+ auto.start();
+ }
+
+ @Override
+ public void loop(){
+ auto.loop();
+ }
+
+ @Override
+ public void stop(){
+ auto.stop();
+ }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlueAutoPointer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlueAutoPointer.java
new file mode 100644
index 00000000000..87874700240
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/BlueAutoPointer.java
@@ -0,0 +1,74 @@
+/*
+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 FITNESSFOR 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.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Blue Auto (Push Beacons)", group="Main")
+@Disabled
+public class BlueAutoPointer extends OpMode {
+
+ BotHardwareOld robot = new BotHardwareOld();
+
+ LineDrive auto = new LineDrive(this, false, false);
+
+ @Override
+ public void init(){
+ auto.init();
+ }
+
+ @Override
+ public void init_loop(){
+ auto.init_loop();
+ }
+
+ @Override
+ public void start(){
+ auto.start();
+ }
+
+ @Override
+ public void loop(){
+ auto.loop();
+ }
+
+ @Override
+ public void stop(){
+ auto.stop();
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/DayZeroAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/DayZeroAuto.java
new file mode 100644
index 00000000000..6e7a25a6c90
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/DayZeroAuto.java
@@ -0,0 +1,73 @@
+/*
+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 FITNESSFOR 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.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Old Auto", group="Main")
+@Disabled
+public class DayZeroAuto extends LinearOpMode {
+
+ BotHardwareOld robot = new BotHardwareOld();
+
+ @Override
+ public void runOpMode() {
+
+ robot.init(this, false);
+
+ // launch first ball
+ robot.launcherMotor.setPower(1.0);
+
+ robot.waitForTick(2000);
+
+ robot.launcherMotor.setPower(0.0);
+
+ // load second ball
+ //robot.lifterMotor.setPower(-1.0);
+
+ robot.waitForTick(5000);
+
+ //robot.lifterMotor.setPower(0.0);
+
+ // launch second ball
+ robot.launcherMotor.setPower(1.0);
+
+ robot.waitForTick(2000);
+
+ robot.launcherMotor.setPower(0.0);
+
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/GyroDriveTestOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/GyroDriveTestOp.java
new file mode 100644
index 00000000000..5b84ad1528f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/GyroDriveTestOp.java
@@ -0,0 +1,145 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+
+
+/**
+ * simple example of using a Step that uses gyro input to drive along a given course for a given time
+ * Created by phanau on 1/3/16 as SensorDriveTestOp, modified 1/22/16 to use Gyro, renamed GyroDriveTestOp 2/16/16.
+ */
+
+
+// simple example sequence that tests either of gyro-based AzimuthCountedDriveStep or AzimuthTimedDriveStep to drive along a square path
+@Autonomous(name="Test: Gyro Drive Test 1", group ="Test")
+@Disabled
+public class GyroDriveTestOp extends OpMode {
+
+ AutoLib.Sequence mSequence; // the root of the sequence tree
+ boolean bDone; // true when the programmed sequence is done
+ DcMotor mMotors[]; // motors, some of which can be null: assumed order is fr, br, fl, bl
+ ModernRoboticsI2cGyro mGyro; // gyro to use for heading information
+ SensorLib.CorrectedMRGyro mCorrGyro; // gyro corrector object
+ boolean bSetup; // true when we're in "setup mode" where joysticks tweak parameters
+ SensorLib.PID mPid; // PID controller for the sequence
+
+ // parameters of the PID controller for this sequence
+ float Kp = 0.035f; // motor power proportional term correction per degree of deviation
+ float Ki = 0.02f; // ... integrator term
+ float Kd = 0; // ... derivative term
+ float KiCutoff = 3.0f; // maximum angle error for which we update integrator
+
+ @Override
+ public void init() {
+ bSetup = false; // start out in Kp/Ki setup mode
+ AutoLib.HardwareFactory mf = null;
+ final boolean debug = false;
+ if (debug)
+ mf = new AutoLib.TestHardwareFactory(this);
+ else
+ mf = new AutoLib.RealHardwareFactory(this);
+
+ // get the motors: depending on the factory we created above, these may be
+ // either dummy motors that just log data or real ones that drive the hardware
+ // assumed order is fr, br, fl, bl
+ mMotors = new DcMotor[4];
+ mMotors[0] = mf.getDcMotor("fr");
+ mMotors[1] = mf.getDcMotor("br");
+ (mMotors[2] = mf.getDcMotor("fl")).setDirection(DcMotor.Direction.REVERSE);
+ (mMotors[3] = mf.getDcMotor("bl")).setDirection(DcMotor.Direction.REVERSE);
+
+ // get hardware gyro
+ mGyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
+
+ // wrap gyro in an object that calibrates it and corrects its output
+ mCorrGyro = new SensorLib.CorrectedMRGyro(mGyro);
+ mCorrGyro.calibrate();
+
+ // create a PID controller for the sequence
+ mPid = new SensorLib.PID(Kp, Ki, Kd, KiCutoff); // make the object that implements PID control algorithm
+
+ // create an autonomous sequence with the steps to drive
+ // several legs of a polygonal course ---
+ float power = 0.25f;
+
+ // create the root Sequence for this autonomous OpMode
+ mSequence = new AutoLib.LinearSequence();
+
+ boolean bUseEncoders = true;
+ if (bUseEncoders) {
+ // add a bunch of encoder-counted "legs" to the sequence - use Gyro heading convention of positive degrees CCW from initial heading
+ int leg = 4000; // motor-encoder count along each leg of the polygon
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 180, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 270, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, -90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, -180, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, -270, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, -90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthCountedDriveStep(this, -180, mCorrGyro, mPid, mMotors, power, leg * 4, true));
+ }
+ else {
+ // add a bunch of timed "legs" to the sequence - use Gyro heading convention of positive degrees CW from initial heading
+ float leg = debug ? 10.0f : 3.0f; // time along each leg of the polygon
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 180, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 270, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, -90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, -180, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, -270, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, 0, mCorrGyro, mPid, mMotors, power, leg * 2, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, -90, mCorrGyro, mPid, mMotors, power, leg, false));
+ mSequence.add(new AutoLib.AzimuthTimedDriveStep(this, -180, mCorrGyro, mPid, mMotors, power, leg * 4, true));
+ }
+
+ // start out not-done
+ bDone = false;
+ }
+
+ @Override
+ public void loop() {
+
+ if (gamepad1.y)
+ bSetup = true; // enter "setup mode" using controller inputs to set Kp and Ki
+ if (gamepad1.x)
+ bSetup = false; // exit "setup mode"
+
+ if (bSetup) { // "setup mode"
+ // adjust PID parameters by joystick inputs
+ Kp -= (gamepad1.left_stick_y * 0.0001f);
+ Ki -= (gamepad1.right_stick_y * 0.0001f);
+ // update the parameters of the PID used by all Steps in this test
+ mPid.setK(Kp, Ki, Kd, KiCutoff);
+ // log updated values to the operator's console
+ telemetry.addData("Kp = ", Kp);
+ telemetry.addData("Ki = ", Ki);
+ return;
+ }
+
+ // until we're done, keep looping through the current Step(s)
+ if (!bDone)
+ bDone = mSequence.loop(); // returns true when we're done
+ else
+ telemetry.addData("sequence finished", "");
+ }
+
+ @Override
+ public void stop() {
+ super.stop();
+ mCorrGyro.stop(); // release the physical sensor(s) we've been using for azimuth data
+ }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/LineDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/LineDrive.java
new file mode 100644
index 00000000000..a11a2aa14f0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/LineDrive.java
@@ -0,0 +1,595 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.UltrasonicSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.libraries.SquirrelyLib;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.FunctionCall;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.SetPower;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.LineFollowLib;
+import org.firstinspires.ftc.teamcode.libraries.OpenCVLib;
+import org.firstinspires.ftc.teamcode.libraries.PushyLib;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.DisplacementSensor;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.FinishSensor;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.HeadingSensor;
+import org.opencv.core.Mat;
+
+import java.util.ArrayList;
+
+
+//I'm just going to pretend I am proud of this
+
+@Autonomous(name="Line Drive", group ="Line Follow")
+@Disabled
+public class LineDrive extends OpenCVLib {
+ AutoLib.Sequence mSequence; // the root of the sequence tree
+ AutoLib.Sequence mShoot;
+ AutoLib.Sequence mDrive;
+ AutoLib.Sequence mPushy;
+ AutoLib.Sequence mCharge;
+ boolean bDone; // true when the programmed sequence is done
+ BotHardwareOld robot; // robot hardware object
+ SensorLib.PID mPid;
+ SensorLib.PID mgPid; // PID controllers for the sequence
+ SensorLib.PID mdPid;
+ SensorLib.PID muPid;
+ SensorLib.PID mGPid;
+ // parameters of the PID controller for this sequence's first part
+ float Kp = 0.05f; // degree heading proportional term correction per degree of deviation
+ //float Ki = 0.02f; // ... integrator term
+ float Ki = 0.00f;
+ float Kd = 0; // ... derivative term
+ float KiCutoff = 3.0f; // maximum angle error for which we update integrator
+
+ // parameters of the PID controller for the heading of the line following
+ float Kp2 = 0.35f;
+ float Ki2 = 0.000f;
+ float Kd2 = 0;
+ float Ki2Cutoff = 0.0f;
+
+ // parameters of the PID controller for the displacement of the line following
+ float Kp3 = 5.00f;
+ float Ki3 = 0.0f;
+ float Kd3 = 0;
+ float Ki3Cutoff = 0.00f;
+
+ // parameters of the PID controller for the ultrasonic sensor driving
+ float Kp4 = 0.025f;
+ float Ki4 = 0.00f;
+ float Kd4 = 0;
+ float Ki4Cutoff = 0.00f;
+
+ //parameters for gyro PID, but cranked up
+ float Kp5 = 0.1f; // degree heading proportional term correction per degree of deviation
+ float Ki5 = 0.05f; // ... integrator term
+ float Kd5 = 0; // ... derivative term
+ float Ki5Cutoff = 3.0f; // maximum angle error for which we update integrator
+
+ final float ultraDistS1 = 15;
+ final float ultraDistS2 = 11;
+
+ //constants for pushy pushy
+ final double pushPos = 1.0;
+ final double pushyTime = 0.5;
+ boolean red = false;
+ final int colorThresh = 200;
+ final float driveTime = 0.10f;
+ final int driveLoopCount = 1;
+ final float pushyPower = 0.2f;
+
+ final int countPerRotation = 280;
+
+ OpMode modePointer = this;
+ boolean mJustShoot = false;
+
+ AutoLib.Timer mWait = new AutoLib.Timer(20);
+
+ public LineDrive(OpMode mode, boolean redColor, boolean justShoot){
+ if(mode != null) modePointer = mode;
+ red = redColor;
+ mJustShoot = justShoot;
+ }
+
+ @Override
+ public void init() {
+ //init hardware objects
+ final boolean debug = false;
+ robot = new BotHardwareOld();
+ robot.init(modePointer, debug, true);
+ robot.setMaxSpeedAll(2500);
+
+ // create a PID controller for the sequence
+ mPid = new SensorLib.PID(Kp, Ki, Kd, KiCutoff);
+ mgPid = new SensorLib.PID(Kp2, Ki2, Kd2, Ki2Cutoff); // make the object that implements PID control algorithm
+ mdPid = new SensorLib.PID(Kp3, Ki3, Kd3, Ki3Cutoff);
+ muPid = new SensorLib.PID(Kp4, Ki4, Kd4, Ki4Cutoff);
+ mGPid = new SensorLib.PID(Kp5, Ki5, Kd5, Ki5Cutoff);
+
+ // create an autonomous sequence with the steps to drive
+ // several legs of a polygonal course ---
+ final float power = 0.5f;
+ initOpenCV();
+ startCamera();
+
+ //start up opencv
+
+ //catch a frame
+ Mat frame = getCameraFrame();
+
+ final OpMode thing = modePointer;
+
+ mSequence = new AutoLib.LinearSequence();
+
+ mShoot = new AutoLib.LinearSequence();
+
+ mShoot.add(new AutoLib.MoveByEncoderStep(robot.getMotorArray(), 0.4f, 850, true));
+ mShoot.add(new AutoLib.LogTimeStep(modePointer, "YAY", 0.5));
+ mShoot.add(new AutoLib.EncoderMotorStep(robot.launcherMotor, 1.0f, 1500, true, modePointer));
+ mShoot.add(new AutoLib.TimedServoStep(robot.ballServo, 0.6f, 0.8, false));
+ mShoot.add(new AutoLib.EncoderMotorStep(robot.launcherMotor, 1.0f, 1500, true, modePointer));
+
+ mSequence.add(mShoot);
+
+ mDrive = new AutoLib.LinearSequence();
+
+ mDrive.add(new AutoLib.MoveByEncoderStep(robot.getMotorArray(), 0.2f, 400, true));
+ mDrive.add(new AutoLib.LogTimeStep(modePointer, "YAY!", 0.1));
+
+ int heading;
+ float turnPower;
+
+ if(red){
+ heading = -90;
+ turnPower = 0.3f;
+ }
+ else {
+ heading = 90;
+ turnPower = 0.3f;
+ }
+
+ //mDrive.add(new AutoLib.GyroTurnStep(modePointer, heading, robot.getNavXHeadingSensor(), robot.getMotorArray(), turnPower, 3.0f, true));
+ mDrive.add(new AutoLib.LogTimeStep(modePointer, "Yay!", 0.1));
+
+ mDrive.add(new AutoLib.RunCodeStep(new FunctionCall() {
+ public void run() {
+ robot.initMotors(thing, debug, true);
+ robot.setMaxSpeedAll(2800);
+ }
+ }));
+
+ //mDrive.add(new AutoLib.SquirrleyAzimuthTimedDriveStep(modePointer, 0, heading, robot.getNavXHeadingSensor(), mPid, robot.getMotorArray(), 0.4f, 1.0f, false));
+ mDrive.add(new SquirrelyLib.SquirrleyAzimuthFinDriveStep(modePointer, 0, heading, robot.getNavXHeadingSensor(), mPid, robot.getMotorArray(), 0.5f, new UltraSensors(robot.distSensorLeft, 70, 2.5), false));
+ //if(red) mDrive.add(new UltraSquirrleyAzimuthTimedDriveStep(modePointer, heading, heading, robot.getNavXHeadingSensor(), new UltraCorrectedDisplacement(modePointer, robot.distSensorLeft, 50), mPid, muPid, robot.getMotorArray(), 0.7f, 1.0f, false));
+ mDrive.add(new UltraSquirrleyAzimuthFinDriveStep(modePointer, heading, heading, robot.navXHeading, new UltraCorrectedDisplacement(modePointer, robot.distSensorLeft, 16, robot.distSensorRight), mPid, muPid, robot.getMotorArray(), 0.4f, new LineSensors(this, frame, 0.0f), true));
+
+ mSequence.add(mDrive);
+
+ mPushy = new AutoLib.LinearSequence();
+
+ //two-stage line follow
+
+ mPushy.add(new AutoLib.RunCodeStep(new FunctionCall() {
+ public void run() {
+ robot.initMotors(thing, debug, true);
+ //robot.setMaxSpeedAll(2500);
+ }
+ }));
+
+
+ //mPushy.add(new AutoLib.GyroTurnStep(modePointer, 0, new LineSensors(this, frame), robot.getMotorArray(), 0.1f, 0.05f, true, true));
+ mPushy.add(new LineFollowLib.LineDriveStep(modePointer, 0, new LineSensors(this, frame), new LineSensors(this, frame), new UltraSensors(robot.distSensorLeft, ultraDistS1, 4.0, robot.distSensorRight), mgPid, mdPid, robot.getMotorArray(), 0.1f, false));
+ mPushy.add(new LineFollowLib.LineDriveStep(modePointer, 0, new LineSensors(this, frame), new LineSensors(this, frame), new UltraSensors(robot.distSensorLeft, ultraDistS2, 4.0, robot.distSensorRight), mgPid, mdPid, robot.getMotorArray(), 0.05f, true));
+
+ /*
+ mPushy.add(new AutoLib.RunCodeStep(new AutoLib.FunctionCall() {
+ public void run() {
+ robot.navX.zeroYaw();
+ }
+ }));
+ */
+
+ //pushy pushy
+ mPushy.add(new PushyLib.pushypushy(modePointer, robot.getMotorArray(), robot.leftSensor, robot.rightSensor, robot.leftServo, robot.rightServo,
+ pushPos, pushyTime, red, colorThresh, pushyPower, driveTime, driveLoopCount));
+
+ mPushy.add(new UltraSquirrleyAzimuthFinDriveStep(modePointer, heading, heading, robot.getNavXHeadingSensor(), new UltraCorrectedDisplacement(modePointer, robot.distSensorLeft, 25, robot.distSensorRight), mPid, muPid, robot.getMotorArray(), 0.4f, new LineSensors(this, frame, 3000.0f), true));
+
+
+ mPushy.add(new AutoLib.RunCodeStep(new FunctionCall() {
+ public void run() {
+ robot.initMotors(thing, debug, true);
+ //robot.setMaxSpeedAll(2500);
+ }
+ }));
+
+ //mPushy.add(new AutoLib.GyroTurnStep(modePointer, 0, new LineSensors(this, frame), robot.getMotorArray(), 0.1f, 0.5f, true, true));
+ mPushy.add(new LineFollowLib.LineDriveStep(modePointer, 0, new LineSensors(this, frame), new LineSensors(this, frame), new UltraSensors(robot.distSensorLeft, ultraDistS1+1, 4.0, robot.distSensorRight), mgPid, mdPid, robot.getMotorArray(), 0.1f, false));
+ mPushy.add(new LineFollowLib.LineDriveStep(modePointer, 0, new LineSensors(this, frame), new LineSensors(this, frame), new UltraSensors(robot.distSensorLeft, ultraDistS2+1, 4.0, robot.distSensorRight), mgPid, mdPid, robot.getMotorArray(), 0.05f, true));
+
+ /*
+ mPushy.add(new AutoLib.RunCodeStep(new AutoLib.FunctionCall() {
+ public void run() {
+ robot.navX.zeroYaw();
+ }
+ }));
+ */
+
+ //pushy pushy
+ mPushy.add(new PushyLib.pushypushy(modePointer, robot.getMotorArray(), robot.leftSensor, robot.rightSensor, robot.leftServo, robot.rightServo,
+ pushPos, pushyTime, red, colorThresh, pushyPower, driveTime, driveLoopCount));
+
+ mSequence.add(mPushy);
+
+ mCharge = new AutoLib.LinearSequence();
+
+ mCharge.add(new AutoLib.RunCodeStep(new FunctionCall() {
+ public void run() {
+ robot.initMotors(thing, debug, true);
+ robot.setMaxSpeedAll(2800);
+ }
+ }));
+
+ mCharge.add(new UltraSquirrleyAzimuthFinDriveStep(modePointer, -heading, heading, robot.getNavXHeadingSensor(), new UltraCorrectedDisplacement(modePointer, robot.distSensorLeft, 30), mPid, muPid, robot.getMotorArray(), 1.0f, new TimerFinish(0.5), false));
+ mCharge.add(new UltraSquirrleyAzimuthFinDriveStep(modePointer, -heading, heading, robot.getNavXHeadingSensor(), new UltraCorrectedDisplacement(modePointer, robot.distSensorLeft, 130), mPid, muPid, robot.getMotorArray(), 1.0f, new TimerFinish(2.25), true));
+
+ mSequence.add(mCharge);
+ // start out not-done
+ bDone = false;
+ }
+
+ @Override
+ public void init_loop(){
+ modePointer.telemetry.addData("NavX Connected", robot.navX.isConnected());
+ modePointer.telemetry.addData("NavX Ready", robot.startNavX());
+ }
+
+ @Override
+ public void start(){
+ robot.navX.zeroYaw();
+ if(mJustShoot) mWait.start();
+ }
+
+ @Override
+ public void loop() {
+ try{
+ if(mJustShoot){
+ // until we're done, keep looping through the current Step(s)
+ if (!bDone && mWait.done())
+ bDone = mShoot.loop(); // returns true when we're done
+ else
+ modePointer.telemetry.addData("sequence finished", "");
+ }
+ else {
+ // until we're done, keep looping through the current Step(s)
+ if (!bDone)
+ bDone = mSequence.loop(); // returns true when we're done
+ else
+ modePointer.telemetry.addData("sequence finished", "");
+ }
+ }
+ catch (Exception e){
+ modePointer.telemetry.addData("Exception: ", e.getMessage());
+ telemetry.update();
+ robot.waitForTick(2000);
+ //init();
+ }
+ }
+
+ @Override
+ public void stop() {
+ super.stop();
+ stopCamera();
+ }
+
+ static public class LineSensors implements HeadingSensor, DisplacementSensor, FinishSensor {
+
+ OpenCVLib mMode;
+ int lineCount;
+ private double lastLinePos;
+ private double lastLineHeading;
+ private int yValStore[] = new int[3];
+ float mWait;
+ ElapsedTime mTime;
+
+ final float dispOffset = 0.1f;
+
+ LineSensors(OpenCVLib mode, Mat calib){
+ mMode = mode;
+ yValStore[0] = calib.rows() / 4;
+ yValStore[1] = calib.rows() / 2;
+ yValStore[2] = (calib.rows() * 3) / 4;
+ reset();
+ }
+
+ LineSensors(OpenCVLib mode, Mat calib, float wait){
+ mMode = mode;
+ yValStore[0] = calib.rows() / 4;
+ yValStore[1] = calib.rows() / 2;
+ yValStore[2] = (calib.rows() * 3) / 4;
+ reset();
+ mWait = wait;
+ }
+
+ public void reset(){
+ lineCount = 0;
+ lastLinePos = 0;
+ lastLineHeading = 0;
+ mWait = 0;
+ }
+
+ //heading sensor code for line following
+ public float getHeading() {
+ Mat frame = mMode.getCameraFrame();
+ //get line displacement
+ double linePos = -LineFollowLib.getAngle(frame, yValStore[0], yValStore[2]);
+ //if it's an error, turn to where line was last
+ if (linePos == LineFollowLib.ERROR_TOO_NOISY) {
+ if (lastLineHeading == 0) return 0.0f;
+ else if (lastLineHeading > 0) return 50.0f;
+ else return -50.0f;
+ } else lastLineHeading = linePos;
+
+ return (float) linePos;
+ }
+
+ public float getDisp() {
+ Mat frame = mMode.getCameraFrame();
+ double linePos = -LineFollowLib.getDisplacment(frame, yValStore[1]);
+ //if it's an error, turn to where line was last
+ if (linePos == LineFollowLib.ERROR_TOO_NOISY) {
+ if(lastLinePos == 0) return 0.0f;
+ else if (lastLinePos > 0) return 0.8f;
+ else return -0.8f;
+ } else lastLinePos = linePos;
+ return (float) linePos + dispOffset;
+ }
+
+ public boolean checkStop() {
+ if(mWait == 0){
+ if (LineFollowLib.getDisplacment(mMode.getCameraFrame(), yValStore[2]) != LineFollowLib.ERROR_TOO_NOISY){
+ //lineCount++;
+ return true;
+ }
+ //else lineCount = 0;
+ //return lineCount >= 3;
+ return false;
+ }
+ else {
+ if(mTime == null) mTime = new ElapsedTime();
+ else if(mTime.milliseconds() > mWait) mWait = 0;
+ return false;
+ }
+ }
+ }
+
+ public static class UltraSensors implements FinishSensor, DisplacementSensor {
+ UltrasonicSensor mSensor;
+ UltrasonicSensor mSensor2;
+ float mDist;
+ float mHeading;
+ double mSeconds;
+ AutoLib.Timer mTime = null;
+
+ UltraSensors(UltrasonicSensor sensor, float dist, double time) {
+ mSensor = sensor;
+ mDist = dist;
+ mSeconds = time;
+ }
+
+ UltraSensors(UltrasonicSensor sensor, float dist, double time, UltrasonicSensor sensor2) {
+ mSensor2 = sensor2;
+ mSensor = sensor;
+ mDist = dist;
+ mSeconds = time;
+ }
+
+ public boolean checkStop() {
+ if(mTime == null) {
+ mTime = new AutoLib.Timer(mSeconds);
+ mTime.start();
+ }
+
+ //get ultrasonic distance
+ double dist;
+ if(mSensor2 == null) dist = mSensor.getUltrasonicLevel();
+ else dist = (mSensor.getUltrasonicLevel() + mSensor2.getUltrasonicLevel()) / 2.0;
+
+ //cutoff ridiculous values
+ if (mSensor.getUltrasonicLevel() > 200 || mSensor.getUltrasonicLevel() < 5 || mSensor2.getUltrasonicLevel() > 200 || mSensor2.getUltrasonicLevel() < 5) return false;
+ //now check if the robot is in range
+ else return (dist < mDist) || mTime.done();
+ }
+
+ public float getDisp(){
+ //get ultrasonic level
+ float dist = (float)mSensor.getUltrasonicLevel();
+
+ //cutoff ridiculous values
+ if (dist > 200) return 100.0f - mDist;
+ if (dist < 5) return 5.0f - mDist;
+ return dist - mDist;
+ }
+ }
+
+ public static class UltraCorrectedDisplacement{
+ private final float Kp = 0.3f;
+ private float mDist;
+ private UltrasonicSensor mSensor;
+ private UltrasonicSensor mSensor2;
+ private OpMode mMode;
+
+ UltraCorrectedDisplacement(OpMode mode, UltrasonicSensor sensor, float dist) {
+ mMode = mode;
+ mSensor = sensor;
+ mDist = dist;
+ }
+
+ UltraCorrectedDisplacement(OpMode mode, UltrasonicSensor sensor, float dist, UltrasonicSensor sensor2) {
+ mSensor2 = sensor2;
+ mMode = mode;
+ mSensor = sensor;
+ mDist = dist;
+ }
+
+ public float getDisp(float angleError){
+ float dist;
+ if(mSensor2 == null) dist = (float)mSensor.getUltrasonicLevel();
+ else dist = (float)((mSensor.getUltrasonicLevel() + mSensor2.getUltrasonicLevel()) / 2.0);
+
+ mMode.telemetry.addData("Ultra", dist);
+
+ if(dist > 200) dist = 200;
+ else if (dist < 5) dist = 5;
+
+ //dist += angleError * Kp;
+
+ return dist - mDist;
+ }
+
+ }
+
+ static public class TimerFinish implements FinishSensor {
+ private AutoLib.Timer mTimer;
+
+ TimerFinish(double seconds){
+ mTimer = new AutoLib.Timer(seconds);
+ }
+
+ public boolean checkStop(){
+ if(!mTimer.isStarted()) mTimer.start();
+ return mTimer.done();
+ }
+
+ }
+
+ static public class UltraSquirrleyGuideStep extends AutoLib.Step {
+ private float mPower; // basic power setting of all 4 motors -- adjusted for steering along path
+ private float mDirection; // compass heading to steer for (-180 .. +180 degrees
+ private float mHeading;
+ private OpMode mOpMode; // needed so we can log output (may be null)
+ private HeadingSensor mGyro; // sensor to use for heading information (e.g. Gyro or Vuforia)
+ private UltraCorrectedDisplacement mUltra;
+ private SensorLib.PID gPid; // proportional–integral–derivative controller (PID controller)
+ private SensorLib.PID dPid;
+ private double mPrevTime; // time of previous loop() call
+ private ArrayList mMotorSteps; // the motor steps we're guiding - assumed order is right ... left ...
+
+ public UltraSquirrleyGuideStep(OpMode mode, float direction, float heading, HeadingSensor gyro, UltraCorrectedDisplacement ultra, SensorLib.PID gpid,
+ SensorLib.PID dpid, ArrayList motorsteps, float power) {
+ mOpMode = mode;
+ mDirection = direction;
+ mHeading = heading;
+ mGyro = gyro;
+ mUltra = ultra;
+ gPid = gpid;
+ dPid = dpid;
+ mMotorSteps = motorsteps;
+ mPower = power;
+ }
+
+ public boolean loop() {
+ super.loop();
+ // initialize previous-time on our first call -> dt will be zero on first call
+ if (firstLoopCall()) {
+ mPrevTime = mOpMode.getRuntime(); // use timer provided by OpMode
+ }
+
+ final float heading = mGyro.getHeading(); // get latest reading from direction sensor
+ // convention is positive angles CCW, wrapping from 359-0
+
+ final float error = SensorLib.Utils.wrapAngle(heading - mHeading); // deviation from desired heading
+ // deviations to left are positive, to right are negative
+
+ // compute delta time since last call -- used for integration time of PID step
+ final double time = mOpMode.getRuntime();
+ final double dt = time - mPrevTime;
+ mPrevTime = time;
+
+ // feed error through PID to get motor power correction value
+ final float gCorrection = -gPid.loop(error, (float) dt);
+
+ final float dCorrection = -dPid.loop(mUltra.getDisp(error), (float) dt);
+
+ //calculate motor powers for fancy wheels
+ SquirrelyLib.MotorPowers mp = SquirrelyLib.GetSquirrelyWheelMotorPowers(mDirection);
+
+ final float leftPower = gCorrection;
+ final float rightPower = -gCorrection;
+ final float frontPower = -dCorrection;
+ final float backPower = -dCorrection;
+
+ final float[] motorPowers = {frontPower + rightPower + (float)mp.Front(),
+ backPower + rightPower + (float)mp.Back(),
+ frontPower + leftPower + (float)mp.Front(),
+ backPower + leftPower + (float)mp.Back()};
+
+ final float scale = AutoLib.scaleMotorFactor(motorPowers);
+
+ //set the powers
+ mMotorSteps.get(0).setPower(motorPowers[0] * scale * mPower);
+ mMotorSteps.get(1).setPower(motorPowers[1] * scale * mPower);
+ mMotorSteps.get(2).setPower(motorPowers[2] * scale * mPower);
+ mMotorSteps.get(3).setPower(motorPowers[3] * scale * mPower);
+
+ // log some data
+ if (mOpMode != null) {
+ mOpMode.telemetry.addData("heading ", heading);
+ //mOpMode.telemetry.addData("front power ", mp.Front());
+ //mOpMode.telemetry.addData("back power ", mp.Back());
+ }
+
+ // guidance step always returns "done" so the CS in which it is embedded completes when
+ // all the motors it's controlling are done
+ return true;
+ }
+ }
+
+ static public class UltraSquirrleyAzimuthTimedDriveStep extends AutoLib.ConcurrentSequence {
+
+ public UltraSquirrleyAzimuthTimedDriveStep(OpMode mode, float direction, float heading, HeadingSensor gyro, UltraCorrectedDisplacement ultra, SensorLib.PID gpid, SensorLib.PID dpid,
+ DcMotor motors[], float power, float time, boolean stop) {
+ // add a concurrent Step to control each motor
+
+ ArrayList steps = new ArrayList();
+ for (DcMotor em : motors)
+ if (em != null) {
+ AutoLib.TimedMotorStep step = new AutoLib.TimedMotorStep(em, 0, time, stop);
+ this.add(step);
+ steps.add(step);
+ }
+
+ // add a concurrent Step to control the motor steps based on gyro input
+ this.preAdd(new UltraSquirrleyGuideStep(mode, direction, heading, gyro, ultra, gpid, dpid, steps, power));
+ }
+
+ // the base class loop function does all we need -- it will return "done" when
+ // all the motors are done.
+ }
+
+ static public class UltraSquirrleyAzimuthFinDriveStep extends AutoLib.ConcurrentSequence {
+
+ public UltraSquirrleyAzimuthFinDriveStep(OpMode mode, float direction, float heading, HeadingSensor gyro, UltraCorrectedDisplacement ultra, SensorLib.PID gpid, SensorLib.PID dpid,
+ DcMotor motors[], float power, FinishSensor fin, boolean stop) {
+ // add a concurrent Step to control each motor
+
+ ArrayList steps = new ArrayList();
+ for (DcMotor em : motors)
+ if (em != null) {
+ AutoLib.DriveUntilStopMotorStep step = new AutoLib.DriveUntilStopMotorStep(em, 0, fin, stop);
+ this.add(step);
+ steps.add(step);
+ }
+
+ // add a concurrent Step to control the motor steps based on gyro input
+ this.preAdd(new UltraSquirrleyGuideStep(mode, direction, heading, gyro, ultra, gpid, dpid, steps, power));
+ }
+
+ // the base class loop function does all we need -- it will return "done" when
+ // all the motors are done.
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/MeccanumVelocityTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/MeccanumVelocityTest.java
new file mode 100644
index 00000000000..1309b8898eb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/MeccanumVelocityTest.java
@@ -0,0 +1,135 @@
+/*
+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 FITNESSFOR 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.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.PushyLib;
+
+/**
+ * This file contains an minimal example of a Linear "OpMode". An OpMode is a 'program' that runs in either
+ * the autonomous or the teleop period of an FTC match. The names of OpModes appear on the menu
+ * of the FTC Driver Station. When an selection is made from the menu, the corresponding OpMode
+ * class is instantiated on the Robot Controller and executed.
+ *
+ * This particular OpMode just executes a basic Tank Drive Teleop for a PushBot
+ * It includes all the skeletal structure that all linear OpModes contain.
+ *
+ * Use Android Studios 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
+ */
+
+@Autonomous(name="Blue Auto", group="Main") // @Autonomous(...) is the other common choice
+@Disabled
+public class MeccanumVelocityTest extends OpMode {
+
+ BotHardwareOld robot = new BotHardwareOld();
+ /* Declare OpMode members. */
+
+ //some constants to make navigating the field easier
+ static final double mmToEncode = 1; //TODO: Find this value
+ static final double inchToMm = 25.4;
+ static final double footToMm = inchToMm * 12;
+ static final double squareToMm = footToMm * 2;
+
+ final float power = 0.2f;
+
+ //constants for pushy pushy
+ final double pushPos = 1.0;
+ final double time = 2.0;
+ final boolean red = false;
+ final int colorThresh = 200;
+ final float driveTime = 0.1f;
+ final int driveLoopCount = 2;
+
+ final boolean debug = false;
+
+ // parameters of the PID controller for this sequence's first part
+ final float Kp = 1.0f; // degree heading proportional term correction per degree of deviation
+ final float Ki = 0.2f; // ... integrator term
+ final float Kd = 0; // ... derivative term
+ final float KiCutoff = 1.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID mdPid = new SensorLib.PID(Kp, Ki, Kd, KiCutoff);
+
+ // create the root Sequence for this autonomous OpMode
+ AutoLib.Sequence mSequence = new AutoLib.LinearSequence();
+
+ // start out not-done
+ boolean bDone = false;
+
+ @Override
+ public void init(){
+ robot.init(this, debug);
+
+ // drive to the first beacon
+ //mSequence.add(new SquirrleyAzimuthTimedDriveStep(this, -90.0f, robot.getNavXHeadingSensor(), mdPid, robot.getMotorArray(), power, 10000, true));
+ //mSequence.add(new SquirrleyAzimuthTimedDriveStep(this, 0, robot.getNavXHeadingSensor(), mdPid, robot.getMotorArray(), power, 5000, true));
+
+ //line following here
+
+ mSequence.add(new AutoLib.MoveByTimeStep(robot.getMotorArray(), 0.0, 0, true));
+
+ //pushy pushy
+ mSequence.add(new PushyLib.pushypushy(this, robot.getMotorArray(), robot.leftSensor, robot.rightSensor, robot.leftServo, robot.rightServo,
+ pushPos, time, red, colorThresh, power, driveTime, driveLoopCount)); //SO. MANY. VARIABLES.
+
+ telemetry.addData("Status", "Initialized");
+ }
+
+ @Override
+ public void start(){
+ robot.startNavX();
+ }
+
+ @Override
+ public void loop(){
+ // until we're done, keep looping through the current Step(s)
+ while (!bDone) {
+ bDone = mSequence.loop(); // returns true when we're done
+ telemetry.update();
+ }
+
+ telemetry.addData("Sequence finished!", "");
+ }
+
+ @Override
+ public void stop(){
+ robot.navX.close();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/OpenCVVuforia.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/OpenCVVuforia.java
new file mode 100644
index 00000000000..0452b807ef3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/OpenCVVuforia.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 FITNESSFOR 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.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.vuforia.Image;
+
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.teamcode.libraries.OpenCVLib;
+import org.firstinspires.ftc.teamcode.libraries.VuforiaLib_FTC2016;
+import org.opencv.android.CameraBridgeViewBase;
+import org.opencv.core.Mat;
+
+import java.nio.ByteBuffer;
+
+/**
+ * This OpMode illustrates the basics of using the VuforiaLib_FTC2016 library to determine
+ * positioning and orientation of robot on the FTC field.
+ */
+
+@Autonomous(name="OpenCV with Vuforia", group ="Test")
+@Disabled
+public class OpenCVVuforia extends OpenCVLib {
+
+ VuforiaLib_FTC2016 mVLib;
+
+ @Override public void init() {
+ /**
+ * Start up Vuforia using VuforiaLib_FTC2016
+ */
+ mVLib = new VuforiaLib_FTC2016();
+ mVLib.init(this, null); // pass it this OpMode (so it can do telemetry output) and use its license key for now
+
+ initOpenCV();
+ }
+
+ @Override public void start()
+ {
+ /** Start tracking the data sets we care about. */
+ mVLib.start();
+ }
+
+ @Override public void loop()
+ {
+ VuforiaLocalizer.CloseableFrame frame;
+ mVLib.loop(true); // update location info and do debug telemetry
+ if(!mVLib.getFrames().isEmpty()){
+ frame = mVLib.getFrames().poll();
+
+ Image img = frame.getImage(0);
+
+ ByteBuffer buf = img.getPixels();
+
+ //this currently throws an unsupportedOperation exception
+ byte[] ray = buf.array();
+ Mat m = new Mat();
+ m.put(0,0,ray);
+ telemetry.addData("Mat Width:", m.width());
+ telemetry.addData("Mat Height:", m.height());
+ }
+
+ }
+
+ @Override public void stop()
+ {
+ mVLib.stop();
+ }
+
+ public Mat onCameraFrame(CameraBridgeViewBase.CvCameraViewFrame frame){
+ return frame.gray();
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/RedAutoPointer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/RedAutoPointer.java
new file mode 100644
index 00000000000..5e1c63c4668
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/RedAutoPointer.java
@@ -0,0 +1,74 @@
+/*
+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 FITNESSFOR 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.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Red Auto (Push Beacons)", group="Main")
+@Disabled
+public class RedAutoPointer extends OpMode {
+
+ BotHardwareOld robot = new BotHardwareOld();
+
+ LineDrive auto = new LineDrive(this, true, false);
+
+ @Override
+ public void init(){
+ auto.init();
+ }
+
+ @Override
+ public void init_loop(){
+ auto.init_loop();
+ }
+
+ @Override
+ public void start(){
+ auto.start();
+ }
+
+ @Override
+ public void loop(){
+ auto.loop();
+ }
+
+ @Override
+ public void stop(){
+ auto.stop();
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/RedPilliar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/RedPilliar.java
new file mode 100644
index 00000000000..1329a6cd745
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/RedPilliar.java
@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.PilliarLib;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.interfaces.HeadingSensor;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 11/13/2017.
+ * Testing file for cryptobox pillar detection using camera
+ */
+
+@Autonomous(name="Red Drive", group="test")
+@Disabled
+public class RedPilliar extends PilliarLib {
+ private static final double BALL_WAIT_SEC = 2.0;//10.0;
+ private static final int PILLIAR_COUNT_INC = 350;
+ private static final int PILLIAR_COUNT_START_BLUE = 40;
+ private static final int PILLIAR_COUNT_START_RED = 250;
+ private static final int PEAK_FIND_WINDOW = 40;
+ private static final int PEAK_FRAME_COUNT = 3;
+
+ protected boolean red = true;
+ protected boolean justDrive = false;
+ private BotHardware bot = new BotHardware(this);
+
+ private BallColor color = BallColor.Undefined;
+ private double startLoop = 0;
+ private boolean firstLoop = false;
+
+ private AutoLib.Sequence mSeq = new AutoLib.LinearSequence();
+
+ //parameters for gyro PID, but cranked up
+ float Kp5 = 3.0f; // degree heading proportional term correction per degree of deviation
+ float Ki5 = 0.0f; // ... integrator term
+ float Kd5 = 0; // ... derivative term
+ float Ki5Cutoff = 0.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID motorPID = new SensorLib.PID(Kp5, Ki5, Kd5, Ki5Cutoff);
+
+ @Override
+ public void init() {
+ initVuforia(true);
+ bot.init();
+ }
+
+ @Override
+ public void start() {
+ //hmmm
+ startTracking();
+ }
+
+ @Override
+ public void loop() {
+ if(startLoop == 0) startLoop = getRuntime();
+ if(getRuntime() - startLoop < BALL_WAIT_SEC && (color == BallColor.Indeterminate || color == BallColor.Undefined)) color = getBallColor();
+ else if(!firstLoop){
+ //init vars
+ int count;
+ if(red) count = PILLIAR_COUNT_START_RED;
+ else count = PILLIAR_COUNT_START_BLUE;
+ final int mul = red ? -1 : 1;
+ //init whacky stick code here
+ AutoLib.Sequence whack = new AutoLib.LinearSequence();
+ //check detection
+ if(color != BallColor.Indeterminate && color != BallColor.Undefined) {
+ if(red) whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed, 0.25, false));
+ else whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue, 0.25, false));
+ whack.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickDown, 0.5, false));
+ //hmmmmm
+ final AutoLib.Step whackLeft;
+ if(red) whackLeft = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed - BotHardware.ServoE.stickBaseSwingSize, 1.0, false);
+ else whackLeft = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue - BotHardware.ServoE.stickBaseSwingSize, 1.0, false);
+ final AutoLib.Step whackRight;
+ if(red) whackRight = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterRed + BotHardware.ServoE.stickBaseSwingSize, 1.0, false);
+ else whackRight = new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseCenterBlue + BotHardware.ServoE.stickBaseSwingSize, 1.0, false);
+ if(color == BallColor.LeftBlue) {
+ if(red) whack.add(whackLeft);
+ else whack.add(whackRight);
+ }
+ else if(color == BallColor.LeftRed) {
+ if(red) whack.add(whackRight);
+ else whack.add(whackLeft);
+ }
+ whack.add(new AutoLib.TimedServoStep(bot.getStick(), BotHardware.ServoE.stickUp, 0.25, false));
+ whack.add(new AutoLib.TimedServoStep(bot.getStickBase(), BotHardware.ServoE.stickBaseHidden, 0.25, false));
+ }
+
+ if(getLastVuMark() != null) {
+ RelicRecoveryVuMark thing = getLastVuMark();
+ //if we're on red it's the far one, else it's the close one
+ if(thing == RelicRecoveryVuMark.LEFT && red) count += PILLIAR_COUNT_INC * 2;
+ //if it's center, always increment
+ if(thing == RelicRecoveryVuMark.CENTER) count += PILLIAR_COUNT_INC;
+ //if its' on the right and we're on blue, go twice
+ if(thing == RelicRecoveryVuMark.RIGHT && !red) count += PILLIAR_COUNT_INC * 2;
+ //telemetry
+ telemetry.addData("Mark", thing.toString());
+ }
+
+ //contruct the block placing seq
+ AutoLib.LinearSequence blockSeq = new AutoLib.LinearSequence();
+
+ blockSeq.add(
+ new AutoLib.RunUntilStopStep(
+ new AutoLib.MoveByTimeStep(bot.getMotorVelocityShimArray(), 135.0f * mul, 5.0f, true),
+ new PeakFindStep(PEAK_FIND_WINDOW, PEAK_FRAME_COUNT, red)
+ )
+ );
+ blockSeq.add(new AutoLib.GyroTurnStep(this, 0, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 45.0f, 360.0f, motorPID, 0.5f, 10, true));
+ blockSeq.add(new PeakHoneStep(bot.getMotorVelocityShimArray(), !red, new SensorLib.PID(-0.15f, -0.05f, 0, 15), 35.0f, 105.0f, 3, this, red));
+
+ blockSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 135.0f * mul, count, true));
+ blockSeq.add(new AutoLib.GyroTurnStep(this, 90, bot.getHeadingSensor(), bot.getMotorVelocityShimArray(), 65.0f, 520.0f, motorPID, 2.0f, 10, true));
+ blockSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 135.0f, 100, true));
+ blockSeq.add(bot.getDropStep());
+ blockSeq.add(new AutoLib.RunUntilStopStep(
+ new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 135.0f, 400, true),
+ new AutoLib.LogTimeStep(this, "huh", 2.0)
+ ));
+ blockSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), -135.0f, 150, true));
+ //blockSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), -135.0f * mul, 100, true));
+ //blockSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 270.0f, 1600, true));
+
+ AutoLib.LinearSequence driveSeq = new AutoLib.LinearSequence();
+
+ driveSeq.add(new AutoLib.MoveByEncoderStep(bot.getMotorVelocityShimArray(), 135.0 * mul, 1500, true));
+
+ //smash it all together
+ mSeq.add(whack);
+ if(!justDrive) mSeq.add(blockSeq);
+ else mSeq.add(driveSeq);
+
+ firstLoop = true;
+ }
+
+ //logs!
+ if(color != null) telemetry.addData("Ball Color", color.toString());
+ if(getLastVuMark() != null) telemetry.addData("VuMark", getLastVuMark().toString());
+
+ if(firstLoop && mSeq.loop()) requestOpModeStop();
+ }
+
+ private class DebugHeading implements HeadingSensor {
+ public float getHeading() {
+ return 0;
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ScottsTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ScottsTeleOP.java
new file mode 100644
index 00000000000..c232f813dae
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ScottsTeleOP.java
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.HardwareRatbot;
+
+/**
+ * Created by bremm on 10/6/2017.
+ */
+
+@TeleOp(name="ratbot2017", group="Ratbot")
+@Disabled
+public class ScottsTeleOP extends OpMode {
+
+ HardwareRatbot robot = new HardwareRatbot();
+
+ @Override
+ public void init() {
+
+ robot.init(hardwareMap);
+
+ telemetry.addData("Say", "Hello Driver"); //
+ }
+
+ @Override
+ public void init_loop() {
+ }
+
+
+ @Override
+ public void start() {
+ }
+
+
+ @Override
+ public void loop() {
+ double left;
+ double right;
+
+ left = -gamepad1.left_stick_y;
+ right = -gamepad1.right_stick_y;
+
+ robot.leftDrive.setPower(left);
+ robot.rightDrive.setPower(right);
+
+ }
+ @Override
+ public void stop() {
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAuto.java
new file mode 100644
index 00000000000..e94836cc03e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAuto.java
@@ -0,0 +1,119 @@
+/*
+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 FITNESSFOR 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.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.SensorLib;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Shoot Auto (Shoot Balls)", group="Main")
+@Disabled
+public class ShootAuto extends OpMode {
+
+ BotHardwareOld robot = new BotHardwareOld();
+
+ AutoLib.Sequence mShoot;
+
+ boolean bDone = false;
+
+ boolean redColor;
+
+ OpMode modePointer;
+
+ // parameters of the PID controller for this sequence's first part
+ float Kp = 0.05f; // degree heading proportional term correction per degree of deviation
+ //float Ki = 0.02f; // ... integrator term
+ float Ki = 0.00f;
+ float Kd = 0; // ... derivative term
+ float KiCutoff = 3.0f; // maximum angle error for which we update integrator
+
+ SensorLib.PID gPid = new SensorLib.PID(Kp, Ki, Kd, KiCutoff);
+
+ public ShootAuto(OpMode mode, boolean isRed){
+ modePointer = mode;
+ redColor = isRed;
+ }
+
+ @Override
+ public void init(){
+ //init hardware objects
+ final boolean debug = false;
+ robot = new BotHardwareOld();
+ robot.init(modePointer, debug, true);
+ robot.setMaxSpeedAll(2500);
+
+ mShoot = new AutoLib.LinearSequence();
+
+ mShoot.add(new AutoLib.LogTimeStep(modePointer, "Wait", 15.0));
+ mShoot.add(new AutoLib.MoveByEncoderStep(robot.getMotorArray(), 0.4f, 1450, true));
+ mShoot.add(new AutoLib.LogTimeStep(modePointer, "YAY", 0.5));
+ mShoot.add(new AutoLib.EncoderMotorStep(robot.launcherMotor, 1.0f, 1500, true, modePointer));
+ mShoot.add(new AutoLib.TimedServoStep(robot.ballServo, 0.6f, 0.8, false));
+ mShoot.add(new AutoLib.EncoderMotorStep(robot.launcherMotor, 1.0f, 1500, true, modePointer));
+
+ int direction;
+ if(redColor) direction = -50;
+ else direction = 50;
+
+ //mShoot.add(new AutoLib.GyroTurnStep(modePointer, direction, robot.getNavXHeadingSensor(), robot.getMotorArray(), 0.4f, 3.0f, true));
+ mShoot.add(new AutoLib.MoveByEncoderStep(robot.getMotorArray(), 0.5f, 2750, true));
+ }
+
+ @Override
+ public void init_loop(){
+ modePointer.telemetry.addData("NavX Connected", robot.navX.isConnected());
+ modePointer.telemetry.addData("NavX Ready", robot.startNavX());
+ }
+
+ @Override
+ public void start(){
+ robot.navX.zeroYaw();
+ }
+
+ @Override
+ public void loop(){
+ // until we're done, keep looping through the current Step(s)
+ if (!bDone)
+ bDone = mShoot.loop(); // returns true when we're done
+ else
+ modePointer.telemetry.addData("sequence finished", "");
+ }
+
+ @Override
+ public void stop(){
+ super.stop();
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAutoBlue.java
new file mode 100644
index 00000000000..e813d03167a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAutoBlue.java
@@ -0,0 +1,42 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+/**
+ * Created by Robotics on 3/7/2017.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Blue Shoot Auto (Shoot Balls)", group="Main")
+@Disabled
+public class ShootAutoBlue extends OpMode {
+ BotHardwareOld robot = new BotHardwareOld();
+
+ ShootAuto auto = new ShootAuto(this, false);
+
+ @Override
+ public void init(){
+ auto.init();
+ }
+
+ @Override
+ public void init_loop(){
+ auto.init_loop();
+ }
+
+ @Override
+ public void start(){
+ auto.start();
+ }
+
+ @Override
+ public void loop(){
+ auto.loop();
+ }
+
+ @Override
+ public void stop(){
+ auto.stop();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAutoRed.java
new file mode 100644
index 00000000000..12fc654e46f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/ShootAutoRed.java
@@ -0,0 +1,42 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+/**
+ * Created by Robotics on 3/7/2017.
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardwareOld;
+
+@Autonomous(name="Red Shoot Auto (Shoot Balls)", group="Main")
+@Disabled
+public class ShootAutoRed extends OpMode {
+ BotHardwareOld robot = new BotHardwareOld();
+
+ ShootAuto auto = new ShootAuto(this, true);
+
+ @Override
+ public void init(){
+ auto.init();
+ }
+
+ @Override
+ public void init_loop(){
+ auto.init_loop();
+ }
+
+ @Override
+ public void start(){
+ auto.start();
+ }
+
+ @Override
+ public void loop(){
+ auto.loop();
+ }
+
+ @Override
+ public void stop(){
+ auto.stop();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDrive1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDrive1.java
new file mode 100644
index 00000000000..afff200a7bf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDrive1.java
@@ -0,0 +1,190 @@
+/* Copyright (c) 2014 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 org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.libraries.SquirrelyLib;
+
+/**
+ * TeleOp Mode
+ *
+ * Enables control of the robot via the gamepad using Squirrely Wheels
+ */
+@TeleOp(name="Test: SquirrelyDrive1", group="Test") // @Autonomous(...) is the other common choice
+@Disabled
+public class SquirrelyDrive1 extends OpMode {
+
+ DcMotor motorFrontRight;
+ DcMotor motorFrontLeft;
+ DcMotor motorBackRight;
+ DcMotor motorBackLeft;
+
+ boolean bDebug = false;
+
+ /**
+ * Constructor
+ */
+ public SquirrelyDrive1() {
+
+ }
+
+ /*
+ * Code to run when the op mode is first enabled goes here
+ *
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
+ */
+ @Override
+ public void init() {
+
+ /*
+ * Use the hardwareMap to get the dc motors and servos by name. Note
+ * that the names of the devices must match the names used when you
+ * configured your robot and created the configuration file.
+ */
+
+ /*
+ * For this test, we assume the following,
+ * There are four motors
+ * "fl" and "bl" are front and back left wheels
+ * "fr" and "br" are front and back right wheels
+ */
+ try {
+ motorFrontRight = hardwareMap.dcMotor.get("front_right");
+ motorFrontLeft = hardwareMap.dcMotor.get("front_left");
+ motorBackRight = hardwareMap.dcMotor.get("back_right");
+ motorBackLeft = hardwareMap.dcMotor.get("back_left");
+ motorFrontRight.setDirection(DcMotor.Direction.REVERSE);
+ motorBackRight.setDirection(DcMotor.Direction.REVERSE);
+ }
+ catch (IllegalArgumentException iax) {
+ bDebug = true;
+ }
+ }
+
+
+ /*
+ * This method will be called repeatedly in a loop
+ *
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
+ */
+ @Override
+ public void loop() {
+
+ // turning drive is on the left stick
+ float tx = gamepad1.left_stick_x;
+ float ty = -gamepad1.left_stick_y; // y is reversed :(
+ float left = (ty + tx/2);
+ float right = (ty - tx/2);
+
+ // clip the right/left values so that the values never exceed +/- 1
+ left = Range.clip(left, -1, 1);
+ right = Range.clip(right, -1, 1);
+
+ // scale the joystick values to make it easier to control
+ // the robot more precisely at slower speeds.
+ //left = (float)scaleInput(left);
+ //right = (float)scaleInput(right);
+
+ // squirrely drive is on the right stick
+ float x = gamepad1.right_stick_x;
+ float y = -gamepad1.right_stick_y; // y is reversed :(
+
+ // clip the values so that the values never exceed +/- 1
+ x = Range.clip(x, -1, 1);
+ y = Range.clip(y, -1, 1);
+
+ // direction angle of stick >> the relative direction we want to move
+ double theta = Math.atan2(-x, y); // stick angle: zero = +y, positive CCW, range +-pi
+ double heading = theta * 180.0/Math.PI; // radians to degrees
+
+ // compute front and back wheel relative speeds needed to go in desired direction
+ SquirrelyLib.MotorPowers mp = SquirrelyLib.GetSquirrelyWheelMotorPowers(heading);
+ double front = mp.Front();
+ double back = mp.Back();
+
+ // power is the magnitude of the stick vector
+ double power = Math.sqrt(x*x + y*y);
+
+ // scale the values by the desired power
+ front *= power;
+ back *= power;
+
+ // combine turning and squirrely drive inputs and
+ double fr = Range.clip(front+right, -1, 1);
+ double br = Range.clip(back+right, -1, 1);
+ double fl = Range.clip(front+left, -1, 1);
+ double bl = Range.clip(back+left, -1, 1);
+
+ // write the values to the motors
+ if (!bDebug) {
+ motorFrontRight.setPower(fr);
+ motorBackRight.setPower(br);
+ motorFrontLeft.setPower(fl);
+ motorBackLeft.setPower(bl);
+ }
+
+ /*
+ * Send telemetry data back to driver station.
+ */
+ telemetry.addData("Text", "*** v1.1 ***");
+ telemetry.addData("front left/right power:", "%.2f %.2f", fl, fr);
+ telemetry.addData("back left/right power:", "%.2f %.2f", bl, br);
+ telemetry.addData("heading:", "%.2f", heading);
+ telemetry.addData("gamepad1:", gamepad1);
+ //telemetry.addData("gamepad2", gamepad2);
+ }
+
+ /*
+ * Code to run when the op mode is first disabled goes here
+ *
+ * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#stop()
+ */
+ @Override
+ public void stop() {
+
+ }
+
+ /*
+ * This method scales the joystick input so for low joystick values, the
+ * scaled value is less than linear. This is to make it easier to drive
+ * the robot more precisely at slower speeds.
+ */
+ double scaleInput(double dVal) {
+ return dVal*dVal*dVal; // maps {-1,1} -> {-1,1}
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDriveTestOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDriveTestOp.java
new file mode 100644
index 00000000000..1887ea6feac
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/SquirrelyDriveTestOp.java
@@ -0,0 +1,99 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.*;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.libraries.AutoLib;
+import org.firstinspires.ftc.teamcode.libraries.SquirrelyLib;
+
+
+/**
+ * simple example of using a Step that makes a bot with "squirrely wheels" drive along a given course
+ * Created by phanau on 10/31/16.
+ */
+
+
+// simple example sequence that tests either of gyro-based AzimuthCountedDriveStep or AzimuthTimedDriveStep to drive along a square path
+@Autonomous(name="Test: Squirrely Drive Test 1", group ="Test")
+@Disabled
+public class SquirrelyDriveTestOp extends OpMode {
+
+ AutoLib.Sequence mSequence; // the root of the sequence tree
+ boolean bDone; // true when the programmed sequence is done
+ DcMotor mMotors[]; // motors, some of which can be null: assumed order is fr, br, fl, bl
+
+ // parameters of the PID controller for this sequence
+ float Kp = 0.035f; // motor power proportional term correction per degree of deviation
+ float Ki = 0.02f; // ... integrator term
+ float Kd = 0; // ... derivative term
+ float KiCutoff = 3.0f; // maximum angle error for which we update integrator
+
+ @Override
+ public void init() {
+ AutoLib.HardwareFactory mf = null;
+ final boolean debug = false;
+ if (debug)
+ mf = new AutoLib.TestHardwareFactory(this);
+ else
+ mf = new AutoLib.RealHardwareFactory(this);
+
+ // get the motors: depending on the factory we created above, these may be
+ // either dummy motors that just log data or real ones that drive the hardware
+ // assumed order is fr, br, fl, bl
+ mMotors = new DcMotor[4];
+ mMotors[0] = mf.getDcMotor("front_right");
+ mMotors[1] = mf.getDcMotor("back_right");
+ (mMotors[2] = mf.getDcMotor("front_left")).setDirection(DcMotor.Direction.REVERSE);
+ (mMotors[3] = mf.getDcMotor("back_left")).setDirection(DcMotor.Direction.REVERSE);
+
+ // create an autonomous sequence with the steps to drive
+ // several legs of a polygonal course ---
+ float power = 1.0f;
+
+ // create the root Sequence for this autonomous OpMode
+ mSequence = new AutoLib.LinearSequence();
+
+ // add a bunch of timed "legs" to the sequence - use Gyro heading convention of positive degrees CCW from initial heading
+ float leg = debug ? 6.0f : 3.0f; // time along each leg of the polygon
+
+ // drive a square
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, -90, power, leg/2, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 0, power, leg, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 90, power, leg, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 180, power, leg, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 270, power, leg/2, false));
+
+ // ... and then a diamond
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, -45, power, leg, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 45, power, leg, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 135, power, leg, false));
+ mSequence.add(new SquirrelyLib.MoveSquirrelyByTimeStep(mMotors, 225, power, leg, false));
+
+ // ... and then sort of a polygonal circle
+ int n = 20; // number of sides
+ for (int i=0; i= 0) ? 1 : -1;
+
+ float frontPower = (ySign * x > 0) ? ySign : (ySign * Math.abs(x) * -2 + ySign);
+ float backPower = (ySign * x < 0) ? ySign : (ySign * Math.abs(x) * -2 + ySign);
+
+ */
+
+
+ int heading = -1;
+ // run drivetrain motors
+ // dpad steering
+ /*
+ if(gamepad1.dpad_up && gamepad1.dpad_left) {
+ heading = 45;
+ //robot.setFrontPower(0.0);
+ //robot.setBackPower(1.0);
+ }
+ else if(gamepad1.dpad_up && gamepad1.dpad_right) {
+ heading = -45;
+ //robot.setFrontPower(1.0);
+ //robot.setBackPower(0.0);
+ }
+ else if(gamepad1.dpad_down && gamepad1.dpad_left) {
+ heading = 135;
+ //robot.setFrontPower(-1.0);
+ //robot.setBackPower(0.0);
+ }
+ else if(gamepad1.dpad_down && gamepad1.dpad_right) {
+ heading = -135;
+ //robot.setFrontPower(0.0);
+ //robot.setBackPower(-1.0);
+ }
+ */
+ if(gamepad1.dpad_up) {
+ heading = 0;
+ //robot.setFrontPower(1.0);
+ //robot.setBackPower(1.0);
+ }
+ else if(gamepad1.dpad_left) {
+ heading = 90;
+ //robot.setFrontPower(-1.0);
+ //robot.setBackPower(1.0);
+ }
+ else if(gamepad1.dpad_right) {
+ heading = -90;
+ //robot.setFrontPower(1.0);
+ //robot.setBackPower(-1.0);
+ }
+ else if(gamepad1.dpad_down) {
+ heading = 180;
+ //robot.setFrontPower(-1.0);
+ //robot.setBackPower(-1.0);
+ }
+ else {
+ // joystick tank steering
+ robot.frontLeftMotor.setPower((-gamepad1.left_stick_y) * powerMult);
+ robot.frontRightMotor.setPower((-gamepad1.right_stick_y) * powerMult);
+ robot.backLeftMotor.setPower((-gamepad1.left_stick_y) * powerMult);
+ robot.backRightMotor.setPower((-gamepad1.right_stick_y) * powerMult);
+ }
+
+ if(heading != -1){
+ //if(!robot.isReversed()) heading = -heading;
+ SquirrelyLib.MotorPowers mp = SquirrelyLib.GetSquirrelyWheelMotorPowers(heading);
+ robot.setFrontPower(mp.Front());
+ robot.setBackPower(mp.Back());
+ }
+
+ // run lifter motor
+ if(gamepad1.left_trigger > 0.2 || gamepad2.left_trigger > 0.2) {
+ robot.sweeperMotor.setPower(-1.0);
+ }
+ else if(gamepad1.right_trigger > 0.2 || gamepad2.right_trigger > 0.2) {
+ robot.sweeperMotor.setPower(1.0);
+ }
+ else {
+ robot.sweeperMotor.setPower(0.0);
+ }
+
+ // run launcher motor
+
+ if(gamepad2.x) {
+ robot.launcherMotor.setPower(1.0);
+ }
+ else {
+ robot.launcherMotor.setPower(0.0);
+ }
+
+ if(gamepad2.b) {
+ robot.ballServo.setPosition(0.6);
+ }
+ else {
+ robot.ballServo.setPosition(0.0);
+ }
+
+ //halfspeed
+ if(gamepad1.left_bumper || gamepad1.right_bumper){
+ powerMult = 0.5;
+ }
+ else {
+ powerMult = 1.0;
+ }
+
+ // toggle button pushers
+ if(!lastBButtonState && gamepad1.b) {
+ leftPusherState *= -1.0;
+ rightPusherState *= -1.0;
+ }
+
+ robot.leftServo.setPosition(leftPusherState);
+ robot.rightServo.setPosition(rightPusherState);
+
+ lastBButtonState = gamepad1.b;
+
+ //lastLeftBumperState = gamepad1.left_bumper;
+ //lastRightBumperState = gamepad1.right_bumper;
+
+ //toggle reversed steering
+ if(!lastAButtonState && gamepad1.a) {
+ robot.initMotors(this, false, !robot.isReversed());
+ if(robot.isReversed()){
+ robot.dim.setLED(0, true);
+ telemetry.addData("Reversed", "True");
+ }
+ else{
+ robot.dim.setLED(0, false);
+ telemetry.addData("Reversed", "False");
+ }
+ }
+ lastAButtonState = gamepad1.a;
+
+ //some fun telemetery
+ /*
+ final double dt = getRuntime() - mLastTime;
+ final double jerkX = (robot.navX.getWorldLinearAccelX() - mLastAccelX) / dt;
+ final double jerkY = (robot.navX.getWorldLinearAccelY() - mLastAccelY) / dt;
+ final double vel = (robot.backLeftMotor.getCurrentPosition() - mLastEncoderCount) / dt;
+ if(mMaxEncoderCount < vel) mMaxEncoderCount = vel;
+ mLastEncoderCount = robot.backLeftMotor.getCurrentPosition();
+ mLastAccelX = robot.navX.getWorldLinearAccelX();
+ mLastAccelY = robot.navX.getWorldLinearAccelY();
+ telemetry.addData("X Axis Accel", robot.navX.getWorldLinearAccelX());
+ telemetry.addData("Y Axis Accel", robot.navX.getWorldLinearAccelY());
+ telemetry.addData("X Axis Jerk", jerkX);
+ telemetry.addData("Y Axis Jerk", jerkY);
+ telemetry.addData("Encode per second", vel);
+ telemetry.addData("Max Encode per second", mMaxEncoderCount);
+ */
+ //}
+ }
+
+ public static class UltraHeading implements HeadingSensor {
+ UltrasonicSensor mLeft;
+ UltrasonicSensor mRight;
+
+ UltraHeading(UltrasonicSensor left, UltrasonicSensor right){
+ mLeft = left;
+ mRight = right;
+ }
+
+ public float getHeading(){
+ return (float)(mRight.getUltrasonicLevel() - mLeft.getUltrasonicLevel());
+ }
+
+ }
+
+ private static class JerkCorrect {
+ OpMode mMode;
+ SensorLib.PID mPid;
+ DcMotor mMotor;
+ AHRS mAccel;
+ double mThresh;
+ double mLastEncoderCount = 0;
+ double mLastTime = 0;
+ double mLastAccel;
+
+ JerkCorrect(OpMode mode, DcMotor motor, AHRS accel, double thresh, SensorLib.PID pid){
+ mMode = mode;
+ mMotor = motor;
+ mAccel = accel;
+ mPid = pid;
+ mThresh = thresh;
+ }
+
+ public double loopCorrection(){
+ if(mLastEncoderCount == 0){
+ mLastEncoderCount = mMotor.getCurrentPosition();
+ mLastTime = mMode.getRuntime();
+ mLastAccel = mAccel.getRawAccelY();
+ }
+
+ //calculate delta time
+ final double dt = mMode.getRuntime() - mLastTime;
+ mLastTime = mMode.getRuntime();
+ //calculate current velocity
+ final double vel = (mMotor.getCurrentPosition() - mLastEncoderCount) / dt;
+ mLastEncoderCount = mMotor.getCurrentPosition();
+ //calculate current jerk, negative and y axis due to sensor mounting
+ final double jerk = -(mAccel.getWorldLinearAccelY() - mLastAccel) / dt;
+ mLastAccel = mAccel.getWorldLinearAccelY();
+
+
+ //check if jerk is beyond thresh
+ if(Math.abs(jerk) > mThresh){
+ //attempt to correct speed towards actual value if so
+ //final double error = vel - (mMotor.getMaxSpeed() * mMotor.getPower());
+ final double error = vel - mMotor.getPower();
+ //and return the correction
+ return -mPid.loop((float)error, (float)dt);
+ }
+
+ //else return 0
+ return 0;
+ }
+ }
+
+ public static float[] getCorrectedSquirrleyMotorPowers(float dt, float mDirection, float mHeading, HeadingSensor mGyro, SensorLib.PID mPid, float mPower) {
+ final float heading = mGyro.getHeading(); // get latest reading from direction sensor
+ // convention is positive angles CCW, wrapping from 359-0
+
+ final float error = SensorLib.Utils.wrapAngle(heading - mHeading); // deviation from desired heading
+ // deviations to left are positive, to right are negative
+
+ // feed error through PID to get motor power correction value
+ final float correction = -mPid.loop(error, dt);
+
+ //calculate motor powers for fancy wheels
+ SquirrelyLib.MotorPowers mp = SquirrelyLib.GetSquirrelyWheelMotorPowers(mDirection);
+
+ final float leftPower = correction;
+ final float rightPower = -correction;
+
+ //fr, br, fl, bl
+ final float[] ret = {
+ (rightPower + (float)mp.Front()) * mPower,
+ (rightPower + (float)mp.Back()) * mPower,
+ (leftPower + (float)mp.Front()) * mPower,
+ (leftPower + (float)mp.Back()) * mPower};
+
+ return ret;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/TeleopWithFilter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/TeleopWithFilter.java
new file mode 100644
index 00000000000..a4a49beb136
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/old2017/TeleopWithFilter.java
@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.opmodes.old2017;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.libraries.FilterLib;
+import org.firstinspires.ftc.teamcode.libraries.hardware.BotHardware;
+
+/**
+ * Created by Noah on 10/27/2017.
+ * Teleop!
+ */
+
+@TeleOp(name="Teleop with Filter")
+@Disabled
+public class TeleopWithFilter extends OpMode {
+ BotHardware bot = new BotHardware(this);
+ FilterLib.MovingWindowFilter leftStick;
+ FilterLib.MovingWindowFilter rightStick;
+
+ private int SIZE = 10;
+
+ private boolean pressedUp = false;
+ private boolean pressedDown = false;
+
+ public void init() {
+ bot.init();
+ }
+
+ public void init_loop() {
+ if(!pressedUp && gamepad1.dpad_up) SIZE++;
+ if(!pressedDown && gamepad1.dpad_down) SIZE--;
+ pressedUp = gamepad1.dpad_up;
+ pressedDown = gamepad1.dpad_down;
+ telemetry.addData("SIZE", SIZE);
+ }
+
+ public void start() {
+ leftStick = new FilterLib.MovingWindowFilter(SIZE);
+ rightStick = new FilterLib.MovingWindowAngleFilter(SIZE);
+ }
+
+ public void loop() {
+ //leftStick.appendValue(gamepad1.left_stick_y);
+ //rightStick.appendValue(gamepad1.right_stick_y);
+
+ bot.setLeftDrive(gamepad1.left_stick_y * 0.5);
+ bot.setRightDrive(gamepad1.right_stick_y * 0.5);
+ }
+
+ public void stop() {
+ bot.stopAll();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BlueJustDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BlueJustDrive.java
new file mode 100644
index 00000000000..4cc2053491b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BlueJustDrive.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.opmodes.pointer;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.teamcode.opmodes.old2017.RedPilliar;
+
+/**
+ * Created by Noah on 12/15/2017.
+ */
+
+@Autonomous(name="Blue No Block", group="test")
+@Disabled
+public class BlueJustDrive extends RedPilliar {
+ public void init() {
+ this.red = false;
+ this.justDrive = true;
+ super.init();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BluePilliar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BluePilliar.java
new file mode 100644
index 00000000000..29d48e571f7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BluePilliar.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.opmodes.pointer;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.teamcode.opmodes.old2017.RedPilliar;
+
+/**
+ * Created by Noah on 12/2/2017.
+ */
+
+@Autonomous(name="Blue Drive", group="test")
+@Disabled
+public class BluePilliar extends RedPilliar {
+ public void init() {
+ this.red = false;
+ super.init();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BlueRearAPDSAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BlueRearAPDSAuto.java
new file mode 100644
index 00000000000..59f00fab757
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/BlueRearAPDSAuto.java
@@ -0,0 +1,18 @@
+/* package org.firstinspires.ftc.teamcode.opmodes.pointer;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.opmodes.old2018.ADPSAuto;
+
+*
+ * Created by Noah on 12/31/2017.
+
+
+@Autonomous(name="Blue Rear Auto", group="test")
+public class BlueRearAPDSAuto extends ADPSAuto {
+ @Override
+ public void init() {
+ red = false;
+ rear = true;
+ super.init();
+ }
+} */
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedAPDSAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedAPDSAuto.java
new file mode 100644
index 00000000000..647d849299d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedAPDSAuto.java
@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.opmodes.pointer;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.opmodes.old2018.ADPSAuto;
+
+/**
+ * Created by Noah on 12/31/2017.
+ */
+
+@Autonomous(name="Red Front Auto", group="test")
+public class RedAPDSAuto extends ADPSAuto {
+ @Override
+ public void init() {
+ red = true;
+ super.init();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedBallDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedBallDrive.java
new file mode 100644
index 00000000000..6fa35a14b2c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedBallDrive.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.opmodes.pointer;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.teamcode.opmodes.old2017.RedPilliar;
+
+/**
+ * Created by Noah on 12/15/2017.
+ */
+
+@Autonomous(name="Red No Block", group="test")
+@Disabled
+public class RedBallDrive extends RedPilliar {
+ public void init(){
+ this.justDrive = true;
+ this.red = true;
+ super.init();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedRearAPDSAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedRearAPDSAuto.java
new file mode 100644
index 00000000000..d3c250ae97b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/RedRearAPDSAuto.java
@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.opmodes.pointer;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.opmodes.old2018.ADPSAuto;
+
+/**
+ * Created by Noah on 12/31/2017.
+ */
+
+@Autonomous(name="Red Rear Auto", group="test")
+public class RedRearAPDSAuto extends ADPSAuto {
+ @Override
+ public void init() {
+ red = true;
+ rear = true;
+ super.init();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/VuMarkAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/VuMarkAutoBlue.java
new file mode 100644
index 00000000000..64d10606f3b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/pointer/VuMarkAutoBlue.java
@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.opmodes.pointer;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.opmodes.demo.VumarkHiJackVideo;
+
+/**
+ * Created by Noah on 11/3/2017.
+ */
+
+@Autonomous(name="Blue Auto", group ="Auto")
+@Disabled
+public class VuMarkAutoBlue extends VumarkHiJackVideo {
+ @Override
+ public void init() {
+ this.RED = false;
+ super.init();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/profiling/FastAsPossible.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/profiling/FastAsPossible.java
new file mode 100644
index 00000000000..4dcb3c1b867
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/profiling/FastAsPossible.java
@@ -0,0 +1,153 @@
+package org.firstinspires.ftc.teamcode.opmodes.profiling;
+
+import android.content.Context;
+import android.hardware.Sensor;
+import android.hardware.SensorEvent;
+import android.hardware.SensorEventListener;
+import android.hardware.SensorManager;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.lynx.commands.core.LynxGetBulkInputDataCommand;
+import com.qualcomm.hardware.lynx.commands.core.LynxGetBulkInputDataResponse;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.opencv.core.Mat;
+
+import java.util.Arrays;
+import java.util.LinkedList;
+import java.util.List;
+
+/**
+ * Created by Noah on 4/25/2018.
+ * Designed for a ratbot using neverest 3.7s
+ * drives forward as fast as possible without slipping the wheels
+ * Direction of accel:
+ *
+ * *----*
+ * | ^ |
+ * | Y | X >
+ * | |
+ * *----*
+ */
+
+@TeleOp(name = "Fast as Possible")
+public class FastAsPossible extends OpMode implements SensorEventListener {
+ private static final double WHEEL_CIRCUMFRENCE_M = DistanceUnit.INCH.toCm(2.875) / 100.0 * Math.PI;
+ private static final double Ka = 0.00001;
+
+ private DcMotorEx leftMotor;
+ private DcMotorEx rightMotor;
+ private LynxModule module;
+ private SensorManager man;
+
+ private float[] lastVal;
+
+ private double lastLeftPos;
+ private double lastRightPos;
+ private double lastLeftVel;
+ private double lastRightVel;
+ private double lastRuntime;
+
+ public void init() {
+ leftMotor = hardwareMap.get(DcMotorEx.class, "l");
+ rightMotor = hardwareMap.get(DcMotorEx.class, "r");
+ module = hardwareMap.get(LynxModule.class, "m");
+
+ man = (SensorManager)hardwareMap.appContext.getSystemService(Context.SENSOR_SERVICE);
+ final Sensor accel = man.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+ man.registerListener(this, accel, 1000);
+
+ /*
+ telemetry.addLine().addData("Accel", new Func() {
+ @Override public String value() {
+ final float[] ray = new float[3];
+ for(float[] val : lastVal) {
+ ray[0] += val[0];
+ ray[1] += val[1];
+ ray[2] += val[2];
+ }
+ ray[0] /= (float)lastVal.size();
+ ray[1] /= (float)lastVal.size();
+ ray[2] /= (float)lastVal.size();
+ lastVal.clear();
+ return Arrays.toString(ray);
+ }
+ });
+ */
+ }
+
+ public void init_loop() {
+
+ }
+
+ public void start() {
+
+ }
+
+ public void loop() {
+ try{
+ LynxGetBulkInputDataCommand command = new LynxGetBulkInputDataCommand(module);
+ LynxGetBulkInputDataResponse resp = command.sendReceive();
+ final double leftPos = resp.getEncoder(1);
+ final double rightPos = resp.getEncoder(0);
+ final double runtime = getRuntime();
+ final double phoneAccel = getAverageAccel();
+ final double delta = runtime - lastRuntime;
+ final double leftVel = resp.getVelocity(1);
+ final double rightVel = resp.getVelocity(0);
+ final double leftAccel = (leftVel - lastLeftVel) / delta * WHEEL_CIRCUMFRENCE_M;
+ final double rightAccel = (rightVel - lastRightVel) / delta * WHEEL_CIRCUMFRENCE_M;
+ //apply kA term to joystick
+ if(gamepad1.left_stick_y > 0) leftMotor.setPower(-gamepad1.left_stick_y - Ka * (phoneAccel - leftAccel));
+ if(gamepad1.right_stick_y > 0) rightMotor.setPower(-gamepad1.right_stick_y - Ka * (phoneAccel - rightAccel));
+ //set
+ lastRuntime = runtime;
+ lastLeftPos = leftPos;
+ lastRightPos = rightPos;
+ lastLeftVel = leftVel;
+ lastRightVel = rightVel;
+ //logs
+ telemetry.addData("Pos Left", leftPos);
+ telemetry.addData("Pos Right", rightPos);
+ telemetry.addData("Vel Left", leftVel);
+ telemetry.addData("Vel Right", rightVel);
+ telemetry.addData("Phone Accel", phoneAccel);
+ telemetry.addData("Left Accel", leftAccel);
+ telemetry.addData("Right Accel", rightAccel);
+
+ }
+ catch (Exception e) {
+ /* hmmmm */
+ }
+
+
+
+
+ }
+
+ public void stop() {
+ leftMotor.setPower(0);
+ rightMotor.setPower(0);
+ man.unregisterListener(this);
+ }
+
+ @Override
+ public void onSensorChanged(SensorEvent sensorEvent) {
+ lastVal = sensorEvent.values;
+ }
+
+ @Override
+ public void onAccuracyChanged(Sensor sensor, int i) {
+
+ }
+
+ private double getAverageAccel() {
+ //return lastVal[2] * 9.80665;
+ return lastVal[2];
+ }
+}
diff --git a/gradle.properties b/gradle.properties
new file mode 100644
index 00000000000..d72861d28d9
--- /dev/null
+++ b/gradle.properties
@@ -0,0 +1 @@
+android.useDeprecatedNdk=true
\ No newline at end of file
diff --git a/import-summary.txt b/import-summary.txt
new file mode 100644
index 00000000000..0fc96693a31
--- /dev/null
+++ b/import-summary.txt
@@ -0,0 +1,244 @@
+ECLIPSE ANDROID PROJECT IMPORT SUMMARY
+======================================
+
+Ignored Files:
+--------------
+The following files were *not* copied into the new Gradle project; you
+should evaluate whether these are still needed in your project and if
+so manually move them:
+
+* javadoc\
+* javadoc\allclasses-frame.html
+* javadoc\allclasses-noframe.html
+* javadoc\constant-values.html
+* javadoc\help-doc.html
+* javadoc\index-all.html
+* javadoc\index.html
+* javadoc\org\
+* javadoc\org\opencv\
+* javadoc\org\opencv\android\
+* javadoc\org\opencv\android\BaseLoaderCallback.html
+* javadoc\org\opencv\android\CameraBridgeViewBase.CvCameraViewFrame.html
+* javadoc\org\opencv\android\CameraBridgeViewBase.CvCameraViewListener.html
+* javadoc\org\opencv\android\CameraBridgeViewBase.CvCameraViewListener2.html
+* javadoc\org\opencv\android\CameraBridgeViewBase.ListItemAccessor.html
+* javadoc\org\opencv\android\CameraBridgeViewBase.html
+* javadoc\org\opencv\android\FpsMeter.html
+* javadoc\org\opencv\android\InstallCallbackInterface.html
+* javadoc\org\opencv\android\JavaCameraView.JavaCameraSizeAccessor.html
+* javadoc\org\opencv\android\JavaCameraView.html
+* javadoc\org\opencv\android\LoaderCallbackInterface.html
+* javadoc\org\opencv\android\OpenCVLoader.html
+* javadoc\org\opencv\android\Utils.html
+* javadoc\org\opencv\android\package-frame.html
+* javadoc\org\opencv\android\package-summary.html
+* javadoc\org\opencv\android\package-tree.html
+* javadoc\org\opencv\calib3d\
+* javadoc\org\opencv\calib3d\Calib3d.html
+* javadoc\org\opencv\calib3d\StereoBM.html
+* javadoc\org\opencv\calib3d\StereoMatcher.html
+* javadoc\org\opencv\calib3d\StereoSGBM.html
+* javadoc\org\opencv\calib3d\package-frame.html
+* javadoc\org\opencv\calib3d\package-summary.html
+* javadoc\org\opencv\calib3d\package-tree.html
+* javadoc\org\opencv\core\
+* javadoc\org\opencv\core\Algorithm.html
+* javadoc\org\opencv\core\Core.MinMaxLocResult.html
+* javadoc\org\opencv\core\Core.html
+* javadoc\org\opencv\core\CvException.html
+* javadoc\org\opencv\core\CvType.html
+* javadoc\org\opencv\core\DMatch.html
+* javadoc\org\opencv\core\KeyPoint.html
+* javadoc\org\opencv\core\Mat.html
+* javadoc\org\opencv\core\MatOfByte.html
+* javadoc\org\opencv\core\MatOfDMatch.html
+* javadoc\org\opencv\core\MatOfDouble.html
+* javadoc\org\opencv\core\MatOfFloat.html
+* javadoc\org\opencv\core\MatOfFloat4.html
+* javadoc\org\opencv\core\MatOfFloat6.html
+* javadoc\org\opencv\core\MatOfInt.html
+* javadoc\org\opencv\core\MatOfInt4.html
+* javadoc\org\opencv\core\MatOfKeyPoint.html
+* javadoc\org\opencv\core\MatOfPoint.html
+* javadoc\org\opencv\core\MatOfPoint2f.html
+* javadoc\org\opencv\core\MatOfPoint3.html
+* javadoc\org\opencv\core\MatOfPoint3f.html
+* javadoc\org\opencv\core\MatOfRect.html
+* javadoc\org\opencv\core\MatOfRect2d.html
+* javadoc\org\opencv\core\Point.html
+* javadoc\org\opencv\core\Point3.html
+* javadoc\org\opencv\core\Range.html
+* javadoc\org\opencv\core\Rect.html
+* javadoc\org\opencv\core\Rect2d.html
+* javadoc\org\opencv\core\RotatedRect.html
+* javadoc\org\opencv\core\Scalar.html
+* javadoc\org\opencv\core\Size.html
+* javadoc\org\opencv\core\TermCriteria.html
+* javadoc\org\opencv\core\TickMeter.html
+* javadoc\org\opencv\core\package-frame.html
+* javadoc\org\opencv\core\package-summary.html
+* javadoc\org\opencv\core\package-tree.html
+* javadoc\org\opencv\dnn\
+* javadoc\org\opencv\dnn\DictValue.html
+* javadoc\org\opencv\dnn\Dnn.html
+* javadoc\org\opencv\dnn\Importer.html
+* javadoc\org\opencv\dnn\Layer.html
+* javadoc\org\opencv\dnn\Net.html
+* javadoc\org\opencv\dnn\package-frame.html
+* javadoc\org\opencv\dnn\package-summary.html
+* javadoc\org\opencv\dnn\package-tree.html
+* javadoc\org\opencv\engine\
+* javadoc\org\opencv\engine\package-frame.html
+* javadoc\org\opencv\engine\package-summary.html
+* javadoc\org\opencv\engine\package-tree.html
+* javadoc\org\opencv\features2d\
+* javadoc\org\opencv\features2d\AKAZE.html
+* javadoc\org\opencv\features2d\AgastFeatureDetector.html
+* javadoc\org\opencv\features2d\BFMatcher.html
+* javadoc\org\opencv\features2d\BOWImgDescriptorExtractor.html
+* javadoc\org\opencv\features2d\BOWKMeansTrainer.html
+* javadoc\org\opencv\features2d\BOWTrainer.html
+* javadoc\org\opencv\features2d\BRISK.html
+* javadoc\org\opencv\features2d\DescriptorExtractor.html
+* javadoc\org\opencv\features2d\DescriptorMatcher.html
+* javadoc\org\opencv\features2d\FastFeatureDetector.html
+* javadoc\org\opencv\features2d\Feature2D.html
+* javadoc\org\opencv\features2d\FeatureDetector.html
+* javadoc\org\opencv\features2d\Features2d.html
+* javadoc\org\opencv\features2d\FlannBasedMatcher.html
+* javadoc\org\opencv\features2d\GFTTDetector.html
+* javadoc\org\opencv\features2d\KAZE.html
+* javadoc\org\opencv\features2d\MSER.html
+* javadoc\org\opencv\features2d\ORB.html
+* javadoc\org\opencv\features2d\Params.html
+* javadoc\org\opencv\features2d\package-frame.html
+* javadoc\org\opencv\features2d\package-summary.html
+* javadoc\org\opencv\features2d\package-tree.html
+* javadoc\org\opencv\imgcodecs\
+* javadoc\org\opencv\imgcodecs\Imgcodecs.html
+* javadoc\org\opencv\imgcodecs\package-frame.html
+* javadoc\org\opencv\imgcodecs\package-summary.html
+* javadoc\org\opencv\imgcodecs\package-tree.html
+* javadoc\org\opencv\imgproc\
+* javadoc\org\opencv\imgproc\CLAHE.html
+* javadoc\org\opencv\imgproc\Imgproc.html
+* javadoc\org\opencv\imgproc\LineSegmentDetector.html
+* javadoc\org\opencv\imgproc\Moments.html
+* javadoc\org\opencv\imgproc\Subdiv2D.html
+* javadoc\org\opencv\imgproc\package-frame.html
+* javadoc\org\opencv\imgproc\package-summary.html
+* javadoc\org\opencv\imgproc\package-tree.html
+* javadoc\org\opencv\ml\
+* javadoc\org\opencv\ml\ANN_MLP.html
+* javadoc\org\opencv\ml\Boost.html
+* javadoc\org\opencv\ml\DTrees.html
+* javadoc\org\opencv\ml\EM.html
+* javadoc\org\opencv\ml\KNearest.html
+* javadoc\org\opencv\ml\LogisticRegression.html
+* javadoc\org\opencv\ml\Ml.html
+* javadoc\org\opencv\ml\NormalBayesClassifier.html
+* javadoc\org\opencv\ml\ParamGrid.html
+* javadoc\org\opencv\ml\RTrees.html
+* javadoc\org\opencv\ml\SVM.html
+* javadoc\org\opencv\ml\SVMSGD.html
+* javadoc\org\opencv\ml\StatModel.html
+* javadoc\org\opencv\ml\TrainData.html
+* javadoc\org\opencv\ml\package-frame.html
+* javadoc\org\opencv\ml\package-summary.html
+* javadoc\org\opencv\ml\package-tree.html
+* javadoc\org\opencv\objdetect\
+* javadoc\org\opencv\objdetect\BaseCascadeClassifier.html
+* javadoc\org\opencv\objdetect\CascadeClassifier.html
+* javadoc\org\opencv\objdetect\HOGDescriptor.html
+* javadoc\org\opencv\objdetect\Objdetect.html
+* javadoc\org\opencv\objdetect\package-frame.html
+* javadoc\org\opencv\objdetect\package-summary.html
+* javadoc\org\opencv\objdetect\package-tree.html
+* javadoc\org\opencv\osgi\
+* javadoc\org\opencv\osgi\OpenCVInterface.html
+* javadoc\org\opencv\osgi\OpenCVNativeLoader.html
+* javadoc\org\opencv\osgi\package-frame.html
+* javadoc\org\opencv\osgi\package-summary.html
+* javadoc\org\opencv\osgi\package-tree.html
+* javadoc\org\opencv\photo\
+* javadoc\org\opencv\photo\AlignExposures.html
+* javadoc\org\opencv\photo\AlignMTB.html
+* javadoc\org\opencv\photo\CalibrateCRF.html
+* javadoc\org\opencv\photo\CalibrateDebevec.html
+* javadoc\org\opencv\photo\CalibrateRobertson.html
+* javadoc\org\opencv\photo\MergeDebevec.html
+* javadoc\org\opencv\photo\MergeExposures.html
+* javadoc\org\opencv\photo\MergeMertens.html
+* javadoc\org\opencv\photo\MergeRobertson.html
+* javadoc\org\opencv\photo\Photo.html
+* javadoc\org\opencv\photo\Tonemap.html
+* javadoc\org\opencv\photo\TonemapDrago.html
+* javadoc\org\opencv\photo\TonemapDurand.html
+* javadoc\org\opencv\photo\TonemapMantiuk.html
+* javadoc\org\opencv\photo\TonemapReinhard.html
+* javadoc\org\opencv\photo\package-frame.html
+* javadoc\org\opencv\photo\package-summary.html
+* javadoc\org\opencv\photo\package-tree.html
+* javadoc\org\opencv\utils\
+* javadoc\org\opencv\utils\Converters.html
+* javadoc\org\opencv\utils\package-frame.html
+* javadoc\org\opencv\utils\package-summary.html
+* javadoc\org\opencv\utils\package-tree.html
+* javadoc\org\opencv\video\
+* javadoc\org\opencv\video\BackgroundSubtractor.html
+* javadoc\org\opencv\video\BackgroundSubtractorKNN.html
+* javadoc\org\opencv\video\BackgroundSubtractorMOG2.html
+* javadoc\org\opencv\video\DenseOpticalFlow.html
+* javadoc\org\opencv\video\DualTVL1OpticalFlow.html
+* javadoc\org\opencv\video\FarnebackOpticalFlow.html
+* javadoc\org\opencv\video\KalmanFilter.html
+* javadoc\org\opencv\video\SparseOpticalFlow.html
+* javadoc\org\opencv\video\SparsePyrLKOpticalFlow.html
+* javadoc\org\opencv\video\Video.html
+* javadoc\org\opencv\video\package-frame.html
+* javadoc\org\opencv\video\package-summary.html
+* javadoc\org\opencv\video\package-tree.html
+* javadoc\org\opencv\videoio\
+* javadoc\org\opencv\videoio\VideoCapture.html
+* javadoc\org\opencv\videoio\VideoWriter.html
+* javadoc\org\opencv\videoio\Videoio.html
+* javadoc\org\opencv\videoio\package-frame.html
+* javadoc\org\opencv\videoio\package-summary.html
+* javadoc\org\opencv\videoio\package-tree.html
+* javadoc\overview-frame.html
+* javadoc\overview-summary.html
+* javadoc\overview-tree.html
+* javadoc\package-list
+* javadoc\resources\
+* javadoc\resources\background.gif
+* javadoc\resources\tab.gif
+* javadoc\resources\titlebar.gif
+* javadoc\resources\titlebar_end.gif
+* javadoc\serialized-form.html
+* javadoc\stylesheet.css
+
+Moved Files:
+------------
+Android Gradle projects use a different directory structure than ADT
+Eclipse projects. Here's how the projects were restructured:
+
+* AndroidManifest.xml => openCVLibrary330\src\main\AndroidManifest.xml
+* lint.xml => openCVLibrary330\lint.xml
+* res\ => openCVLibrary330\src\main\res\
+* src\ => openCVLibrary330\src\main\java\
+* src\org\opencv\engine\OpenCVEngineInterface.aidl => openCVLibrary330\src\main\aidl\org\opencv\engine\OpenCVEngineInterface.aidl
+
+Next Steps:
+-----------
+You can now build the project. The Gradle project needs network
+connectivity to download dependencies.
+
+Bugs:
+-----
+If for some reason your project does not build, and you determine that
+it is due to a bug or limitation of the Eclipse to Gradle importer,
+please file a bug at http://b.android.com with category
+Component-Tools.
+
+(This import summary is for your information only, and can be deleted
+after import once you are satisfied with the results.)
diff --git a/libs/armeabi-v7a/libopencv_calib3d.a b/libs/armeabi-v7a/libopencv_calib3d.a
new file mode 100644
index 00000000000..1f06a50bbb1
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_calib3d.a differ
diff --git a/libs/armeabi-v7a/libopencv_core.a b/libs/armeabi-v7a/libopencv_core.a
new file mode 100644
index 00000000000..2801ee08484
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_core.a differ
diff --git a/libs/armeabi-v7a/libopencv_dnn.a b/libs/armeabi-v7a/libopencv_dnn.a
new file mode 100644
index 00000000000..358e8fd3ed9
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_dnn.a differ
diff --git a/libs/armeabi-v7a/libopencv_features2d.a b/libs/armeabi-v7a/libopencv_features2d.a
new file mode 100644
index 00000000000..01ed76acfbe
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_features2d.a differ
diff --git a/libs/armeabi-v7a/libopencv_flann.a b/libs/armeabi-v7a/libopencv_flann.a
new file mode 100644
index 00000000000..612ef1da799
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_flann.a differ
diff --git a/libs/armeabi-v7a/libopencv_highgui.a b/libs/armeabi-v7a/libopencv_highgui.a
new file mode 100644
index 00000000000..b5a9710fc0c
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_highgui.a differ
diff --git a/libs/armeabi-v7a/libopencv_imgcodecs.a b/libs/armeabi-v7a/libopencv_imgcodecs.a
new file mode 100644
index 00000000000..e39238d65e4
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_imgcodecs.a differ
diff --git a/libs/armeabi-v7a/libopencv_imgproc.a b/libs/armeabi-v7a/libopencv_imgproc.a
new file mode 100644
index 00000000000..c1e211cfef7
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_imgproc.a differ
diff --git a/libs/armeabi-v7a/libopencv_java3.so b/libs/armeabi-v7a/libopencv_java3.so
new file mode 100644
index 00000000000..82b92390ae7
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_java3.so differ
diff --git a/libs/armeabi-v7a/libopencv_ml.a b/libs/armeabi-v7a/libopencv_ml.a
new file mode 100644
index 00000000000..8c8973ae994
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_ml.a differ
diff --git a/libs/armeabi-v7a/libopencv_objdetect.a b/libs/armeabi-v7a/libopencv_objdetect.a
new file mode 100644
index 00000000000..06b88a0ee5d
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_objdetect.a differ
diff --git a/libs/armeabi-v7a/libopencv_photo.a b/libs/armeabi-v7a/libopencv_photo.a
new file mode 100644
index 00000000000..30923d0a118
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_photo.a differ
diff --git a/libs/armeabi-v7a/libopencv_shape.a b/libs/armeabi-v7a/libopencv_shape.a
new file mode 100644
index 00000000000..5c98796dda0
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_shape.a differ
diff --git a/libs/armeabi-v7a/libopencv_stitching.a b/libs/armeabi-v7a/libopencv_stitching.a
new file mode 100644
index 00000000000..20c795925d1
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_stitching.a differ
diff --git a/libs/armeabi-v7a/libopencv_superres.a b/libs/armeabi-v7a/libopencv_superres.a
new file mode 100644
index 00000000000..a6807a52f76
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_superres.a differ
diff --git a/libs/armeabi-v7a/libopencv_video.a b/libs/armeabi-v7a/libopencv_video.a
new file mode 100644
index 00000000000..e23e6b15ab2
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_video.a differ
diff --git a/libs/armeabi-v7a/libopencv_videoio.a b/libs/armeabi-v7a/libopencv_videoio.a
new file mode 100644
index 00000000000..fffb1d74247
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_videoio.a differ
diff --git a/libs/armeabi-v7a/libopencv_videostab.a b/libs/armeabi-v7a/libopencv_videostab.a
new file mode 100644
index 00000000000..365d852b5a6
Binary files /dev/null and b/libs/armeabi-v7a/libopencv_videostab.a differ
diff --git a/libs/navx_ftc-release.aar b/libs/navx_ftc-release.aar
new file mode 100644
index 00000000000..0ae9d96858c
Binary files /dev/null and b/libs/navx_ftc-release.aar differ
diff --git a/openCVLibrary330/build.gradle b/openCVLibrary330/build.gradle
new file mode 100644
index 00000000000..20096872683
--- /dev/null
+++ b/openCVLibrary330/build.gradle
@@ -0,0 +1,18 @@
+apply plugin: 'com.android.library'
+
+android {
+ compileSdkVersion 23
+ buildToolsVersion "25.0.3"
+
+ defaultConfig {
+ minSdkVersion 19
+ targetSdkVersion 23
+ }
+
+ buildTypes {
+ release {
+ minifyEnabled false
+ proguardFiles getDefaultProguardFile('proguard-android.txt'), 'proguard-rules.txt'
+ }
+ }
+}
diff --git a/openCVLibrary330/lint.xml b/openCVLibrary330/lint.xml
new file mode 100644
index 00000000000..6a5c9c61900
--- /dev/null
+++ b/openCVLibrary330/lint.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/openCVLibrary330/src/main/AndroidManifest.xml b/openCVLibrary330/src/main/AndroidManifest.xml
new file mode 100644
index 00000000000..664c637551a
--- /dev/null
+++ b/openCVLibrary330/src/main/AndroidManifest.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
diff --git a/openCVLibrary330/src/main/aidl/org/opencv/engine/OpenCVEngineInterface.aidl b/openCVLibrary330/src/main/aidl/org/opencv/engine/OpenCVEngineInterface.aidl
new file mode 100644
index 00000000000..21fe5f716ba
--- /dev/null
+++ b/openCVLibrary330/src/main/aidl/org/opencv/engine/OpenCVEngineInterface.aidl
@@ -0,0 +1,33 @@
+package org.opencv.engine;
+
+/**
+* Class provides a Java interface for OpenCV Engine Service. It's synchronous with native OpenCVEngine class.
+*/
+interface OpenCVEngineInterface
+{
+ /**
+ * @return Returns service version.
+ */
+ int getEngineVersion();
+
+ /**
+ * Finds an installed OpenCV library.
+ * @param OpenCV version.
+ * @return Returns path to OpenCV native libs or an empty string if OpenCV can not be found.
+ */
+ String getLibPathByVersion(String version);
+
+ /**
+ * Tries to install defined version of OpenCV from Google Play Market.
+ * @param OpenCV version.
+ * @return Returns true if installation was successful or OpenCV package has been already installed.
+ */
+ boolean installVersion(String version);
+
+ /**
+ * Returns list of libraries in loading order, separated by semicolon.
+ * @param OpenCV version.
+ * @return Returns names of OpenCV libraries, separated by semicolon.
+ */
+ String getLibraryList(String version);
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/AsyncServiceHelper.java b/openCVLibrary330/src/main/java/org/opencv/android/AsyncServiceHelper.java
new file mode 100644
index 00000000000..ebee2bb7f59
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/AsyncServiceHelper.java
@@ -0,0 +1,391 @@
+package org.opencv.android;
+
+import java.io.File;
+import java.util.StringTokenizer;
+
+import org.opencv.core.Core;
+import org.opencv.engine.OpenCVEngineInterface;
+
+import android.content.ComponentName;
+import android.content.Context;
+import android.content.Intent;
+import android.content.ServiceConnection;
+import android.net.Uri;
+import android.os.IBinder;
+import android.os.RemoteException;
+import android.util.Log;
+
+class AsyncServiceHelper
+{
+ public static boolean initOpenCV(String Version, final Context AppContext,
+ final LoaderCallbackInterface Callback)
+ {
+ AsyncServiceHelper helper = new AsyncServiceHelper(Version, AppContext, Callback);
+ Intent intent = new Intent("org.opencv.engine.BIND");
+ intent.setPackage("org.opencv.engine");
+ if (AppContext.bindService(intent, helper.mServiceConnection, Context.BIND_AUTO_CREATE))
+ {
+ return true;
+ }
+ else
+ {
+ AppContext.unbindService(helper.mServiceConnection);
+ InstallService(AppContext, Callback);
+ return false;
+ }
+ }
+
+ protected AsyncServiceHelper(String Version, Context AppContext, LoaderCallbackInterface Callback)
+ {
+ mOpenCVersion = Version;
+ mUserAppCallback = Callback;
+ mAppContext = AppContext;
+ }
+
+ protected static final String TAG = "OpenCVManager/Helper";
+ protected static final int MINIMUM_ENGINE_VERSION = 2;
+ protected OpenCVEngineInterface mEngineService;
+ protected LoaderCallbackInterface mUserAppCallback;
+ protected String mOpenCVersion;
+ protected Context mAppContext;
+ protected static boolean mServiceInstallationProgress = false;
+ protected static boolean mLibraryInstallationProgress = false;
+
+ protected static boolean InstallServiceQuiet(Context context)
+ {
+ boolean result = true;
+ try
+ {
+ Intent intent = new Intent(Intent.ACTION_VIEW, Uri.parse(OPEN_CV_SERVICE_URL));
+ intent.addFlags(Intent.FLAG_ACTIVITY_NEW_TASK);
+ context.startActivity(intent);
+ }
+ catch(Exception e)
+ {
+ result = false;
+ }
+
+ return result;
+ }
+
+ protected static void InstallService(final Context AppContext, final LoaderCallbackInterface Callback)
+ {
+ if (!mServiceInstallationProgress)
+ {
+ Log.d(TAG, "Request new service installation");
+ InstallCallbackInterface InstallQuery = new InstallCallbackInterface() {
+ private LoaderCallbackInterface mUserAppCallback = Callback;
+ public String getPackageName()
+ {
+ return "OpenCV Manager";
+ }
+ public void install() {
+ Log.d(TAG, "Trying to install OpenCV Manager via Google Play");
+
+ boolean result = InstallServiceQuiet(AppContext);
+ if (result)
+ {
+ mServiceInstallationProgress = true;
+ Log.d(TAG, "Package installation started");
+ }
+ else
+ {
+ Log.d(TAG, "OpenCV package was not installed!");
+ int Status = LoaderCallbackInterface.MARKET_ERROR;
+ Log.d(TAG, "Init finished with status " + Status);
+ Log.d(TAG, "Unbind from service");
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(Status);
+ }
+ }
+
+ public void cancel()
+ {
+ Log.d(TAG, "OpenCV library installation was canceled");
+ int Status = LoaderCallbackInterface.INSTALL_CANCELED;
+ Log.d(TAG, "Init finished with status " + Status);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(Status);
+ }
+
+ public void wait_install()
+ {
+ Log.e(TAG, "Instalation was not started! Nothing to wait!");
+ }
+ };
+
+ Callback.onPackageInstall(InstallCallbackInterface.NEW_INSTALLATION, InstallQuery);
+ }
+ else
+ {
+ Log.d(TAG, "Waiting current installation process");
+ InstallCallbackInterface WaitQuery = new InstallCallbackInterface() {
+ private LoaderCallbackInterface mUserAppCallback = Callback;
+ public String getPackageName()
+ {
+ return "OpenCV Manager";
+ }
+ public void install()
+ {
+ Log.e(TAG, "Nothing to install we just wait current installation");
+ }
+ public void cancel()
+ {
+ Log.d(TAG, "Waiting for OpenCV canceled by user");
+ mServiceInstallationProgress = false;
+ int Status = LoaderCallbackInterface.INSTALL_CANCELED;
+ Log.d(TAG, "Init finished with status " + Status);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(Status);
+ }
+ public void wait_install()
+ {
+ InstallServiceQuiet(AppContext);
+ }
+ };
+
+ Callback.onPackageInstall(InstallCallbackInterface.INSTALLATION_PROGRESS, WaitQuery);
+ }
+ }
+
+ /**
+ * URL of OpenCV Manager page on Google Play Market.
+ */
+ protected static final String OPEN_CV_SERVICE_URL = "market://details?id=org.opencv.engine";
+
+ protected ServiceConnection mServiceConnection = new ServiceConnection()
+ {
+ public void onServiceConnected(ComponentName className, IBinder service)
+ {
+ Log.d(TAG, "Service connection created");
+ mEngineService = OpenCVEngineInterface.Stub.asInterface(service);
+ if (null == mEngineService)
+ {
+ Log.d(TAG, "OpenCV Manager Service connection fails. May be service was not installed?");
+ InstallService(mAppContext, mUserAppCallback);
+ }
+ else
+ {
+ mServiceInstallationProgress = false;
+ try
+ {
+ if (mEngineService.getEngineVersion() < MINIMUM_ENGINE_VERSION)
+ {
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.INCOMPATIBLE_MANAGER_VERSION);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.INCOMPATIBLE_MANAGER_VERSION);
+ return;
+ }
+
+ Log.d(TAG, "Trying to get library path");
+ String path = mEngineService.getLibPathByVersion(mOpenCVersion);
+ if ((null == path) || (path.length() == 0))
+ {
+ if (!mLibraryInstallationProgress)
+ {
+ InstallCallbackInterface InstallQuery = new InstallCallbackInterface() {
+ public String getPackageName()
+ {
+ return "OpenCV library";
+ }
+ public void install() {
+ Log.d(TAG, "Trying to install OpenCV lib via Google Play");
+ try
+ {
+ if (mEngineService.installVersion(mOpenCVersion))
+ {
+ mLibraryInstallationProgress = true;
+ Log.d(TAG, "Package installation started");
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ }
+ else
+ {
+ Log.d(TAG, "OpenCV package was not installed!");
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.MARKET_ERROR);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.MARKET_ERROR);
+ }
+ } catch (RemoteException e) {
+ e.printStackTrace();;
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.INIT_FAILED);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.INIT_FAILED);
+ }
+ }
+ public void cancel() {
+ Log.d(TAG, "OpenCV library installation was canceled");
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.INSTALL_CANCELED);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.INSTALL_CANCELED);
+ }
+ public void wait_install() {
+ Log.e(TAG, "Installation was not started! Nothing to wait!");
+ }
+ };
+
+ mUserAppCallback.onPackageInstall(InstallCallbackInterface.NEW_INSTALLATION, InstallQuery);
+ }
+ else
+ {
+ InstallCallbackInterface WaitQuery = new InstallCallbackInterface() {
+ public String getPackageName()
+ {
+ return "OpenCV library";
+ }
+
+ public void install() {
+ Log.e(TAG, "Nothing to install we just wait current installation");
+ }
+ public void cancel()
+ {
+ Log.d(TAG, "OpenCV library installation was canceled");
+ mLibraryInstallationProgress = false;
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.INSTALL_CANCELED);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.INSTALL_CANCELED);
+ }
+ public void wait_install() {
+ Log.d(TAG, "Waiting for current installation");
+ try
+ {
+ if (!mEngineService.installVersion(mOpenCVersion))
+ {
+ Log.d(TAG, "OpenCV package was not installed!");
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.MARKET_ERROR);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.MARKET_ERROR);
+ }
+ else
+ {
+ Log.d(TAG, "Wating for package installation");
+ }
+
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+
+ } catch (RemoteException e) {
+ e.printStackTrace();
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.INIT_FAILED);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.INIT_FAILED);
+ }
+ }
+ };
+
+ mUserAppCallback.onPackageInstall(InstallCallbackInterface.INSTALLATION_PROGRESS, WaitQuery);
+ }
+ return;
+ }
+ else
+ {
+ Log.d(TAG, "Trying to get library list");
+ mLibraryInstallationProgress = false;
+ String libs = mEngineService.getLibraryList(mOpenCVersion);
+ Log.d(TAG, "Library list: \"" + libs + "\"");
+ Log.d(TAG, "First attempt to load libs");
+ int status;
+ if (initOpenCVLibs(path, libs))
+ {
+ Log.d(TAG, "First attempt to load libs is OK");
+ String eol = System.getProperty("line.separator");
+ for (String str : Core.getBuildInformation().split(eol))
+ Log.i(TAG, str);
+
+ status = LoaderCallbackInterface.SUCCESS;
+ }
+ else
+ {
+ Log.d(TAG, "First attempt to load libs fails");
+ status = LoaderCallbackInterface.INIT_FAILED;
+ }
+
+ Log.d(TAG, "Init finished with status " + status);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(status);
+ }
+ }
+ catch (RemoteException e)
+ {
+ e.printStackTrace();
+ Log.d(TAG, "Init finished with status " + LoaderCallbackInterface.INIT_FAILED);
+ Log.d(TAG, "Unbind from service");
+ mAppContext.unbindService(mServiceConnection);
+ Log.d(TAG, "Calling using callback");
+ mUserAppCallback.onManagerConnected(LoaderCallbackInterface.INIT_FAILED);
+ }
+ }
+ }
+
+ public void onServiceDisconnected(ComponentName className)
+ {
+ mEngineService = null;
+ }
+ };
+
+ private boolean loadLibrary(String AbsPath)
+ {
+ boolean result = true;
+
+ Log.d(TAG, "Trying to load library " + AbsPath);
+ try
+ {
+ System.load(AbsPath);
+ Log.d(TAG, "OpenCV libs init was ok!");
+ }
+ catch(UnsatisfiedLinkError e)
+ {
+ Log.d(TAG, "Cannot load library \"" + AbsPath + "\"");
+ e.printStackTrace();
+ result &= false;
+ }
+
+ return result;
+ }
+
+ private boolean initOpenCVLibs(String Path, String Libs)
+ {
+ Log.d(TAG, "Trying to init OpenCV libs");
+ if ((null != Path) && (Path.length() != 0))
+ {
+ boolean result = true;
+ if ((null != Libs) && (Libs.length() != 0))
+ {
+ Log.d(TAG, "Trying to load libs by dependency list");
+ StringTokenizer splitter = new StringTokenizer(Libs, ";");
+ while(splitter.hasMoreTokens())
+ {
+ String AbsLibraryPath = Path + File.separator + splitter.nextToken();
+ result &= loadLibrary(AbsLibraryPath);
+ }
+ }
+ else
+ {
+ // If the dependencies list is not defined or empty.
+ String AbsLibraryPath = Path + File.separator + "libopencv_java3.so";
+ result &= loadLibrary(AbsLibraryPath);
+ }
+
+ return result;
+ }
+ else
+ {
+ Log.d(TAG, "Library path \"" + Path + "\" is empty");
+ return false;
+ }
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/BaseLoaderCallback.java b/openCVLibrary330/src/main/java/org/opencv/android/BaseLoaderCallback.java
new file mode 100644
index 00000000000..0b8aeedc6a2
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/BaseLoaderCallback.java
@@ -0,0 +1,141 @@
+package org.opencv.android;
+
+import android.app.Activity;
+import android.app.AlertDialog;
+import android.content.Context;
+import android.content.DialogInterface;
+import android.content.DialogInterface.OnClickListener;
+import android.util.Log;
+
+/**
+ * Basic implementation of LoaderCallbackInterface.
+ */
+public abstract class BaseLoaderCallback implements LoaderCallbackInterface {
+
+ public BaseLoaderCallback(Context AppContext) {
+ mAppContext = AppContext;
+ }
+
+ public void onManagerConnected(int status)
+ {
+ switch (status)
+ {
+ /** OpenCV initialization was successful. **/
+ case LoaderCallbackInterface.SUCCESS:
+ {
+ /** Application must override this method to handle successful library initialization. **/
+ } break;
+ /** OpenCV loader can not start Google Play Market. **/
+ case LoaderCallbackInterface.MARKET_ERROR:
+ {
+ Log.e(TAG, "Package installation failed!");
+ AlertDialog MarketErrorMessage = new AlertDialog.Builder(mAppContext).create();
+ MarketErrorMessage.setTitle("OpenCV Manager");
+ MarketErrorMessage.setMessage("Package installation failed!");
+ MarketErrorMessage.setCancelable(false); // This blocks the 'BACK' button
+ MarketErrorMessage.setButton(AlertDialog.BUTTON_POSITIVE, "OK", new OnClickListener() {
+ public void onClick(DialogInterface dialog, int which) {
+ finish();
+ }
+ });
+ MarketErrorMessage.show();
+ } break;
+ /** Package installation has been canceled. **/
+ case LoaderCallbackInterface.INSTALL_CANCELED:
+ {
+ Log.d(TAG, "OpenCV library installation was canceled by user");
+ finish();
+ } break;
+ /** Application is incompatible with this version of OpenCV Manager. Possibly, a service update is required. **/
+ case LoaderCallbackInterface.INCOMPATIBLE_MANAGER_VERSION:
+ {
+ Log.d(TAG, "OpenCV Manager Service is uncompatible with this app!");
+ AlertDialog IncomatibilityMessage = new AlertDialog.Builder(mAppContext).create();
+ IncomatibilityMessage.setTitle("OpenCV Manager");
+ IncomatibilityMessage.setMessage("OpenCV Manager service is incompatible with this app. Try to update it via Google Play.");
+ IncomatibilityMessage.setCancelable(false); // This blocks the 'BACK' button
+ IncomatibilityMessage.setButton(AlertDialog.BUTTON_POSITIVE, "OK", new OnClickListener() {
+ public void onClick(DialogInterface dialog, int which) {
+ finish();
+ }
+ });
+ IncomatibilityMessage.show();
+ } break;
+ /** Other status, i.e. INIT_FAILED. **/
+ default:
+ {
+ Log.e(TAG, "OpenCV loading failed!");
+ AlertDialog InitFailedDialog = new AlertDialog.Builder(mAppContext).create();
+ InitFailedDialog.setTitle("OpenCV error");
+ InitFailedDialog.setMessage("OpenCV was not initialised correctly. Application will be shut down");
+ InitFailedDialog.setCancelable(false); // This blocks the 'BACK' button
+ InitFailedDialog.setButton(AlertDialog.BUTTON_POSITIVE, "OK", new OnClickListener() {
+
+ public void onClick(DialogInterface dialog, int which) {
+ finish();
+ }
+ });
+
+ InitFailedDialog.show();
+ } break;
+ }
+ }
+
+ public void onPackageInstall(final int operation, final InstallCallbackInterface callback)
+ {
+ switch (operation)
+ {
+ case InstallCallbackInterface.NEW_INSTALLATION:
+ {
+ AlertDialog InstallMessage = new AlertDialog.Builder(mAppContext).create();
+ InstallMessage.setTitle("Package not found");
+ InstallMessage.setMessage(callback.getPackageName() + " package was not found! Try to install it?");
+ InstallMessage.setCancelable(false); // This blocks the 'BACK' button
+ InstallMessage.setButton(AlertDialog.BUTTON_POSITIVE, "Yes", new OnClickListener()
+ {
+ public void onClick(DialogInterface dialog, int which)
+ {
+ callback.install();
+ }
+ });
+
+ InstallMessage.setButton(AlertDialog.BUTTON_NEGATIVE, "No", new OnClickListener() {
+
+ public void onClick(DialogInterface dialog, int which)
+ {
+ callback.cancel();
+ }
+ });
+
+ InstallMessage.show();
+ } break;
+ case InstallCallbackInterface.INSTALLATION_PROGRESS:
+ {
+ AlertDialog WaitMessage = new AlertDialog.Builder(mAppContext).create();
+ WaitMessage.setTitle("OpenCV is not ready");
+ WaitMessage.setMessage("Installation is in progress. Wait or exit?");
+ WaitMessage.setCancelable(false); // This blocks the 'BACK' button
+ WaitMessage.setButton(AlertDialog.BUTTON_POSITIVE, "Wait", new OnClickListener() {
+ public void onClick(DialogInterface dialog, int which) {
+ callback.wait_install();
+ }
+ });
+ WaitMessage.setButton(AlertDialog.BUTTON_NEGATIVE, "Exit", new OnClickListener() {
+ public void onClick(DialogInterface dialog, int which) {
+ callback.cancel();
+ }
+ });
+
+ WaitMessage.show();
+ } break;
+ }
+ }
+
+ void finish()
+ {
+ ((Activity) mAppContext).finish();
+ }
+
+ protected Context mAppContext;
+ private final static String TAG = "OpenCVLoader/BaseLoaderCallback";
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/Camera2Renderer.java b/openCVLibrary330/src/main/java/org/opencv/android/Camera2Renderer.java
new file mode 100644
index 00000000000..408214057a9
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/Camera2Renderer.java
@@ -0,0 +1,302 @@
+package org.opencv.android;
+
+import java.util.Arrays;
+import java.util.concurrent.Semaphore;
+import java.util.concurrent.TimeUnit;
+import android.annotation.TargetApi;
+import android.content.Context;
+import android.graphics.SurfaceTexture;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCaptureSession;
+import android.hardware.camera2.CameraCharacteristics;
+import android.hardware.camera2.CameraDevice;
+import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CaptureRequest;
+import android.hardware.camera2.params.StreamConfigurationMap;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.util.Log;
+import android.util.Size;
+import android.view.Surface;
+
+@TargetApi(21)
+public class Camera2Renderer extends CameraGLRendererBase {
+
+ protected final String LOGTAG = "Camera2Renderer";
+ private CameraDevice mCameraDevice;
+ private CameraCaptureSession mCaptureSession;
+ private CaptureRequest.Builder mPreviewRequestBuilder;
+ private String mCameraID;
+ private Size mPreviewSize = new Size(-1, -1);
+
+ private HandlerThread mBackgroundThread;
+ private Handler mBackgroundHandler;
+ private Semaphore mCameraOpenCloseLock = new Semaphore(1);
+
+ Camera2Renderer(CameraGLSurfaceView view) {
+ super(view);
+ }
+
+ @Override
+ protected void doStart() {
+ Log.d(LOGTAG, "doStart");
+ startBackgroundThread();
+ super.doStart();
+ }
+
+
+ @Override
+ protected void doStop() {
+ Log.d(LOGTAG, "doStop");
+ super.doStop();
+ stopBackgroundThread();
+ }
+
+ boolean cacPreviewSize(final int width, final int height) {
+ Log.i(LOGTAG, "cacPreviewSize: "+width+"x"+height);
+ if(mCameraID == null) {
+ Log.e(LOGTAG, "Camera isn't initialized!");
+ return false;
+ }
+ CameraManager manager = (CameraManager) mView.getContext()
+ .getSystemService(Context.CAMERA_SERVICE);
+ try {
+ CameraCharacteristics characteristics = manager
+ .getCameraCharacteristics(mCameraID);
+ StreamConfigurationMap map = characteristics
+ .get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP);
+ int bestWidth = 0, bestHeight = 0;
+ float aspect = (float)width / height;
+ for (Size psize : map.getOutputSizes(SurfaceTexture.class)) {
+ int w = psize.getWidth(), h = psize.getHeight();
+ Log.d(LOGTAG, "trying size: "+w+"x"+h);
+ if ( width >= w && height >= h &&
+ bestWidth <= w && bestHeight <= h &&
+ Math.abs(aspect - (float)w/h) < 0.2 ) {
+ bestWidth = w;
+ bestHeight = h;
+ }
+ }
+ Log.i(LOGTAG, "best size: "+bestWidth+"x"+bestHeight);
+ if( bestWidth == 0 || bestHeight == 0 ||
+ mPreviewSize.getWidth() == bestWidth &&
+ mPreviewSize.getHeight() == bestHeight )
+ return false;
+ else {
+ mPreviewSize = new Size(bestWidth, bestHeight);
+ return true;
+ }
+ } catch (CameraAccessException e) {
+ Log.e(LOGTAG, "cacPreviewSize - Camera Access Exception");
+ } catch (IllegalArgumentException e) {
+ Log.e(LOGTAG, "cacPreviewSize - Illegal Argument Exception");
+ } catch (SecurityException e) {
+ Log.e(LOGTAG, "cacPreviewSize - Security Exception");
+ }
+ return false;
+ }
+
+ @Override
+ protected void openCamera(int id) {
+ Log.i(LOGTAG, "openCamera");
+ CameraManager manager = (CameraManager) mView.getContext().getSystemService(Context.CAMERA_SERVICE);
+ try {
+ String camList[] = manager.getCameraIdList();
+ if(camList.length == 0) {
+ Log.e(LOGTAG, "Error: camera isn't detected.");
+ return;
+ }
+ if(id == CameraBridgeViewBase.CAMERA_ID_ANY) {
+ mCameraID = camList[0];
+ } else {
+ for (String cameraID : camList) {
+ CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraID);
+ if( id == CameraBridgeViewBase.CAMERA_ID_BACK &&
+ characteristics.get(CameraCharacteristics.LENS_FACING) == CameraCharacteristics.LENS_FACING_BACK ||
+ id == CameraBridgeViewBase.CAMERA_ID_FRONT &&
+ characteristics.get(CameraCharacteristics.LENS_FACING) == CameraCharacteristics.LENS_FACING_FRONT) {
+ mCameraID = cameraID;
+ break;
+ }
+ }
+ }
+ if(mCameraID != null) {
+ if (!mCameraOpenCloseLock.tryAcquire(2500, TimeUnit.MILLISECONDS)) {
+ throw new RuntimeException(
+ "Time out waiting to lock camera opening.");
+ }
+ Log.i(LOGTAG, "Opening camera: " + mCameraID);
+ manager.openCamera(mCameraID, mStateCallback, mBackgroundHandler);
+ }
+ } catch (CameraAccessException e) {
+ Log.e(LOGTAG, "OpenCamera - Camera Access Exception");
+ } catch (IllegalArgumentException e) {
+ Log.e(LOGTAG, "OpenCamera - Illegal Argument Exception");
+ } catch (SecurityException e) {
+ Log.e(LOGTAG, "OpenCamera - Security Exception");
+ } catch (InterruptedException e) {
+ Log.e(LOGTAG, "OpenCamera - Interrupted Exception");
+ }
+ }
+
+ @Override
+ protected void closeCamera() {
+ Log.i(LOGTAG, "closeCamera");
+ try {
+ mCameraOpenCloseLock.acquire();
+ if (null != mCaptureSession) {
+ mCaptureSession.close();
+ mCaptureSession = null;
+ }
+ if (null != mCameraDevice) {
+ mCameraDevice.close();
+ mCameraDevice = null;
+ }
+ } catch (InterruptedException e) {
+ throw new RuntimeException("Interrupted while trying to lock camera closing.", e);
+ } finally {
+ mCameraOpenCloseLock.release();
+ }
+ }
+
+ private final CameraDevice.StateCallback mStateCallback = new CameraDevice.StateCallback() {
+
+ @Override
+ public void onOpened(CameraDevice cameraDevice) {
+ mCameraDevice = cameraDevice;
+ mCameraOpenCloseLock.release();
+ createCameraPreviewSession();
+ }
+
+ @Override
+ public void onDisconnected(CameraDevice cameraDevice) {
+ cameraDevice.close();
+ mCameraDevice = null;
+ mCameraOpenCloseLock.release();
+ }
+
+ @Override
+ public void onError(CameraDevice cameraDevice, int error) {
+ cameraDevice.close();
+ mCameraDevice = null;
+ mCameraOpenCloseLock.release();
+ }
+
+ };
+
+ private void createCameraPreviewSession() {
+ int w=mPreviewSize.getWidth(), h=mPreviewSize.getHeight();
+ Log.i(LOGTAG, "createCameraPreviewSession("+w+"x"+h+")");
+ if(w<0 || h<0)
+ return;
+ try {
+ mCameraOpenCloseLock.acquire();
+ if (null == mCameraDevice) {
+ mCameraOpenCloseLock.release();
+ Log.e(LOGTAG, "createCameraPreviewSession: camera isn't opened");
+ return;
+ }
+ if (null != mCaptureSession) {
+ mCameraOpenCloseLock.release();
+ Log.e(LOGTAG, "createCameraPreviewSession: mCaptureSession is already started");
+ return;
+ }
+ if(null == mSTexture) {
+ mCameraOpenCloseLock.release();
+ Log.e(LOGTAG, "createCameraPreviewSession: preview SurfaceTexture is null");
+ return;
+ }
+ mSTexture.setDefaultBufferSize(w, h);
+
+ Surface surface = new Surface(mSTexture);
+
+ mPreviewRequestBuilder = mCameraDevice
+ .createCaptureRequest(CameraDevice.TEMPLATE_PREVIEW);
+ mPreviewRequestBuilder.addTarget(surface);
+
+ mCameraDevice.createCaptureSession(Arrays.asList(surface),
+ new CameraCaptureSession.StateCallback() {
+ @Override
+ public void onConfigured( CameraCaptureSession cameraCaptureSession) {
+ mCaptureSession = cameraCaptureSession;
+ try {
+ mPreviewRequestBuilder.set(CaptureRequest.CONTROL_AF_MODE, CaptureRequest.CONTROL_AF_MODE_CONTINUOUS_PICTURE);
+ mPreviewRequestBuilder.set(CaptureRequest.CONTROL_AE_MODE, CaptureRequest.CONTROL_AE_MODE_ON_AUTO_FLASH);
+
+ mCaptureSession.setRepeatingRequest(mPreviewRequestBuilder.build(), null, mBackgroundHandler);
+ Log.i(LOGTAG, "CameraPreviewSession has been started");
+ } catch (CameraAccessException e) {
+ Log.e(LOGTAG, "createCaptureSession failed");
+ }
+ mCameraOpenCloseLock.release();
+ }
+
+ @Override
+ public void onConfigureFailed(
+ CameraCaptureSession cameraCaptureSession) {
+ Log.e(LOGTAG, "createCameraPreviewSession failed");
+ mCameraOpenCloseLock.release();
+ }
+ }, mBackgroundHandler);
+ } catch (CameraAccessException e) {
+ Log.e(LOGTAG, "createCameraPreviewSession");
+ } catch (InterruptedException e) {
+ throw new RuntimeException(
+ "Interrupted while createCameraPreviewSession", e);
+ }
+ finally {
+ //mCameraOpenCloseLock.release();
+ }
+ }
+
+ private void startBackgroundThread() {
+ Log.i(LOGTAG, "startBackgroundThread");
+ stopBackgroundThread();
+ mBackgroundThread = new HandlerThread("CameraBackground");
+ mBackgroundThread.start();
+ mBackgroundHandler = new Handler(mBackgroundThread.getLooper());
+ }
+
+ private void stopBackgroundThread() {
+ Log.i(LOGTAG, "stopBackgroundThread");
+ if(mBackgroundThread == null)
+ return;
+ mBackgroundThread.quitSafely();
+ try {
+ mBackgroundThread.join();
+ mBackgroundThread = null;
+ mBackgroundHandler = null;
+ } catch (InterruptedException e) {
+ Log.e(LOGTAG, "stopBackgroundThread");
+ }
+ }
+
+ @Override
+ protected void setCameraPreviewSize(int width, int height) {
+ Log.i(LOGTAG, "setCameraPreviewSize("+width+"x"+height+")");
+ if(mMaxCameraWidth > 0 && mMaxCameraWidth < width) width = mMaxCameraWidth;
+ if(mMaxCameraHeight > 0 && mMaxCameraHeight < height) height = mMaxCameraHeight;
+ try {
+ mCameraOpenCloseLock.acquire();
+
+ boolean needReconfig = cacPreviewSize(width, height);
+ mCameraWidth = mPreviewSize.getWidth();
+ mCameraHeight = mPreviewSize.getHeight();
+
+ if( !needReconfig ) {
+ mCameraOpenCloseLock.release();
+ return;
+ }
+ if (null != mCaptureSession) {
+ Log.d(LOGTAG, "closing existing previewSession");
+ mCaptureSession.close();
+ mCaptureSession = null;
+ }
+ mCameraOpenCloseLock.release();
+ createCameraPreviewSession();
+ } catch (InterruptedException e) {
+ mCameraOpenCloseLock.release();
+ throw new RuntimeException("Interrupted while setCameraPreviewSize.", e);
+ }
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/CameraBridgeViewBase.java b/openCVLibrary330/src/main/java/org/opencv/android/CameraBridgeViewBase.java
new file mode 100644
index 00000000000..54871057e2d
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/CameraBridgeViewBase.java
@@ -0,0 +1,495 @@
+package org.opencv.android;
+
+import java.util.List;
+
+import org.opencv.BuildConfig;
+import org.opencv.R;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+
+import android.app.Activity;
+import android.app.AlertDialog;
+import android.content.Context;
+import android.content.DialogInterface;
+import android.content.res.TypedArray;
+import android.graphics.Bitmap;
+import android.graphics.Canvas;
+import android.graphics.Rect;
+import android.util.AttributeSet;
+import android.util.Log;
+import android.view.SurfaceHolder;
+import android.view.SurfaceView;
+
+/**
+ * This is a basic class, implementing the interaction with Camera and OpenCV library.
+ * The main responsibility of it - is to control when camera can be enabled, process the frame,
+ * call external listener to make any adjustments to the frame and then draw the resulting
+ * frame to the screen.
+ * The clients shall implement CvCameraViewListener.
+ */
+public abstract class CameraBridgeViewBase extends SurfaceView implements SurfaceHolder.Callback {
+
+ private static final String TAG = "CameraBridge";
+ private static final int MAX_UNSPECIFIED = -1;
+ private static final int STOPPED = 0;
+ private static final int STARTED = 1;
+
+ private int mState = STOPPED;
+ private Bitmap mCacheBitmap;
+ private CvCameraViewListener2 mListener;
+ private boolean mSurfaceExist;
+ private final Object mSyncObject = new Object();
+
+ protected int mFrameWidth;
+ protected int mFrameHeight;
+ protected int mMaxHeight;
+ protected int mMaxWidth;
+ protected float mScale = 0;
+ protected int mPreviewFormat = RGBA;
+ protected int mCameraIndex = CAMERA_ID_ANY;
+ protected boolean mEnabled;
+ protected FpsMeter mFpsMeter = null;
+
+ public static final int CAMERA_ID_ANY = -1;
+ public static final int CAMERA_ID_BACK = 99;
+ public static final int CAMERA_ID_FRONT = 98;
+ public static final int RGBA = 1;
+ public static final int GRAY = 2;
+
+ public CameraBridgeViewBase(Context context, int cameraId) {
+ super(context);
+ mCameraIndex = cameraId;
+ getHolder().addCallback(this);
+ mMaxWidth = MAX_UNSPECIFIED;
+ mMaxHeight = MAX_UNSPECIFIED;
+ }
+
+ public CameraBridgeViewBase(Context context, AttributeSet attrs) {
+ super(context, attrs);
+
+ int count = attrs.getAttributeCount();
+ Log.d(TAG, "Attr count: " + Integer.valueOf(count));
+
+ TypedArray styledAttrs = getContext().obtainStyledAttributes(attrs, R.styleable.CameraBridgeViewBase);
+ if (styledAttrs.getBoolean(R.styleable.CameraBridgeViewBase_show_fps, false))
+ enableFpsMeter();
+
+ mCameraIndex = styledAttrs.getInt(R.styleable.CameraBridgeViewBase_camera_id, -1);
+
+ getHolder().addCallback(this);
+ mMaxWidth = MAX_UNSPECIFIED;
+ mMaxHeight = MAX_UNSPECIFIED;
+ styledAttrs.recycle();
+ }
+
+ /**
+ * Sets the camera index
+ * @param cameraIndex new camera index
+ */
+ public void setCameraIndex(int cameraIndex) {
+ this.mCameraIndex = cameraIndex;
+ }
+
+ public interface CvCameraViewListener {
+ /**
+ * This method is invoked when camera preview has started. After this method is invoked
+ * the frames will start to be delivered to client via the onCameraFrame() callback.
+ * @param width - the width of the frames that will be delivered
+ * @param height - the height of the frames that will be delivered
+ */
+ public void onCameraViewStarted(int width, int height);
+
+ /**
+ * This method is invoked when camera preview has been stopped for some reason.
+ * No frames will be delivered via onCameraFrame() callback after this method is called.
+ */
+ public void onCameraViewStopped();
+
+ /**
+ * This method is invoked when delivery of the frame needs to be done.
+ * The returned values - is a modified frame which needs to be displayed on the screen.
+ * TODO: pass the parameters specifying the format of the frame (BPP, YUV or RGB and etc)
+ */
+ public Mat onCameraFrame(Mat inputFrame);
+ }
+
+ public interface CvCameraViewListener2 {
+ /**
+ * This method is invoked when camera preview has started. After this method is invoked
+ * the frames will start to be delivered to client via the onCameraFrame() callback.
+ * @param width - the width of the frames that will be delivered
+ * @param height - the height of the frames that will be delivered
+ */
+ public void onCameraViewStarted(int width, int height);
+
+ /**
+ * This method is invoked when camera preview has been stopped for some reason.
+ * No frames will be delivered via onCameraFrame() callback after this method is called.
+ */
+ public void onCameraViewStopped();
+
+ /**
+ * This method is invoked when delivery of the frame needs to be done.
+ * The returned values - is a modified frame which needs to be displayed on the screen.
+ * TODO: pass the parameters specifying the format of the frame (BPP, YUV or RGB and etc)
+ */
+ public Mat onCameraFrame(CvCameraViewFrame inputFrame);
+ };
+
+ protected class CvCameraViewListenerAdapter implements CvCameraViewListener2 {
+ public CvCameraViewListenerAdapter(CvCameraViewListener oldStypeListener) {
+ mOldStyleListener = oldStypeListener;
+ }
+
+ public void onCameraViewStarted(int width, int height) {
+ mOldStyleListener.onCameraViewStarted(width, height);
+ }
+
+ public void onCameraViewStopped() {
+ mOldStyleListener.onCameraViewStopped();
+ }
+
+ public Mat onCameraFrame(CvCameraViewFrame inputFrame) {
+ Mat result = null;
+ switch (mPreviewFormat) {
+ case RGBA:
+ result = mOldStyleListener.onCameraFrame(inputFrame.rgba());
+ break;
+ case GRAY:
+ result = mOldStyleListener.onCameraFrame(inputFrame.gray());
+ break;
+ default:
+ Log.e(TAG, "Invalid frame format! Only RGBA and Gray Scale are supported!");
+ };
+
+ return result;
+ }
+
+ public void setFrameFormat(int format) {
+ mPreviewFormat = format;
+ }
+
+ private int mPreviewFormat = RGBA;
+ private CvCameraViewListener mOldStyleListener;
+ };
+
+ /**
+ * This class interface is abstract representation of single frame from camera for onCameraFrame callback
+ * Attention: Do not use objects, that represents this interface out of onCameraFrame callback!
+ */
+ public interface CvCameraViewFrame {
+
+ /**
+ * This method returns RGBA Mat with frame
+ */
+ public Mat rgba();
+
+ /**
+ * This method returns single channel gray scale Mat with frame
+ */
+ public Mat gray();
+ };
+
+ public void surfaceChanged(SurfaceHolder arg0, int arg1, int arg2, int arg3) {
+ Log.d(TAG, "call surfaceChanged event");
+ synchronized(mSyncObject) {
+ if (!mSurfaceExist) {
+ mSurfaceExist = true;
+ checkCurrentState();
+ } else {
+ /** Surface changed. We need to stop camera and restart with new parameters */
+ /* Pretend that old surface has been destroyed */
+ mSurfaceExist = false;
+ checkCurrentState();
+ /* Now use new surface. Say we have it now */
+ mSurfaceExist = true;
+ checkCurrentState();
+ }
+ }
+ }
+
+ public void surfaceCreated(SurfaceHolder holder) {
+ /* Do nothing. Wait until surfaceChanged delivered */
+ }
+
+ public void surfaceDestroyed(SurfaceHolder holder) {
+ synchronized(mSyncObject) {
+ mSurfaceExist = false;
+ checkCurrentState();
+ }
+ }
+
+ /**
+ * This method is provided for clients, so they can enable the camera connection.
+ * The actual onCameraViewStarted callback will be delivered only after both this method is called and surface is available
+ */
+ public void enableView() {
+ synchronized(mSyncObject) {
+ mEnabled = true;
+ checkCurrentState();
+ }
+ }
+
+ /**
+ * This method is provided for clients, so they can disable camera connection and stop
+ * the delivery of frames even though the surface view itself is not destroyed and still stays on the scren
+ */
+ public void disableView() {
+ synchronized(mSyncObject) {
+ mEnabled = false;
+ checkCurrentState();
+ }
+ }
+
+ /**
+ * This method enables label with fps value on the screen
+ */
+ public void enableFpsMeter() {
+ if (mFpsMeter == null) {
+ mFpsMeter = new FpsMeter();
+ mFpsMeter.setResolution(mFrameWidth, mFrameHeight);
+ }
+ }
+
+ public void disableFpsMeter() {
+ mFpsMeter = null;
+ }
+
+ /**
+ *
+ * @param listener
+ */
+
+ public void setCvCameraViewListener(CvCameraViewListener2 listener) {
+ mListener = listener;
+ }
+
+ public void setCvCameraViewListener(CvCameraViewListener listener) {
+ CvCameraViewListenerAdapter adapter = new CvCameraViewListenerAdapter(listener);
+ adapter.setFrameFormat(mPreviewFormat);
+ mListener = adapter;
+ }
+
+ /**
+ * This method sets the maximum size that camera frame is allowed to be. When selecting
+ * size - the biggest size which less or equal the size set will be selected.
+ * As an example - we set setMaxFrameSize(200,200) and we have 176x152 and 320x240 sizes. The
+ * preview frame will be selected with 176x152 size.
+ * This method is useful when need to restrict the size of preview frame for some reason (for example for video recording)
+ * @param maxWidth - the maximum width allowed for camera frame.
+ * @param maxHeight - the maximum height allowed for camera frame
+ */
+ public void setMaxFrameSize(int maxWidth, int maxHeight) {
+ mMaxWidth = maxWidth;
+ mMaxHeight = maxHeight;
+ }
+
+ public void SetCaptureFormat(int format)
+ {
+ mPreviewFormat = format;
+ if (mListener instanceof CvCameraViewListenerAdapter) {
+ CvCameraViewListenerAdapter adapter = (CvCameraViewListenerAdapter) mListener;
+ adapter.setFrameFormat(mPreviewFormat);
+ }
+ }
+
+ /**
+ * Called when mSyncObject lock is held
+ */
+ private void checkCurrentState() {
+ Log.d(TAG, "call checkCurrentState");
+ int targetState;
+
+ if (mEnabled && mSurfaceExist && getVisibility() == VISIBLE) {
+ targetState = STARTED;
+ } else {
+ targetState = STOPPED;
+ }
+
+ if (targetState != mState) {
+ /* The state change detected. Need to exit the current state and enter target state */
+ processExitState(mState);
+ mState = targetState;
+ processEnterState(mState);
+ }
+ }
+
+ private void processEnterState(int state) {
+ Log.d(TAG, "call processEnterState: " + state);
+ switch(state) {
+ case STARTED:
+ onEnterStartedState();
+ if (mListener != null) {
+ mListener.onCameraViewStarted(mFrameWidth, mFrameHeight);
+ }
+ break;
+ case STOPPED:
+ onEnterStoppedState();
+ if (mListener != null) {
+ mListener.onCameraViewStopped();
+ }
+ break;
+ };
+ }
+
+ private void processExitState(int state) {
+ Log.d(TAG, "call processExitState: " + state);
+ switch(state) {
+ case STARTED:
+ onExitStartedState();
+ break;
+ case STOPPED:
+ onExitStoppedState();
+ break;
+ };
+ }
+
+ private void onEnterStoppedState() {
+ /* nothing to do */
+ }
+
+ private void onExitStoppedState() {
+ /* nothing to do */
+ }
+
+ // NOTE: The order of bitmap constructor and camera connection is important for android 4.1.x
+ // Bitmap must be constructed before surface
+ private void onEnterStartedState() {
+ Log.d(TAG, "call onEnterStartedState");
+ /* Connect camera */
+ if (!connectCamera(getWidth(), getHeight())) {
+ AlertDialog ad = new AlertDialog.Builder(getContext()).create();
+ ad.setCancelable(false); // This blocks the 'BACK' button
+ ad.setMessage("It seems that you device does not support camera (or it is locked). Application will be closed.");
+ ad.setButton(DialogInterface.BUTTON_NEUTRAL, "OK", new DialogInterface.OnClickListener() {
+ public void onClick(DialogInterface dialog, int which) {
+ dialog.dismiss();
+ ((Activity) getContext()).finish();
+ }
+ });
+ ad.show();
+
+ }
+ }
+
+ private void onExitStartedState() {
+ disconnectCamera();
+ if (mCacheBitmap != null) {
+ mCacheBitmap.recycle();
+ }
+ }
+
+ /**
+ * This method shall be called by the subclasses when they have valid
+ * object and want it to be delivered to external client (via callback) and
+ * then displayed on the screen.
+ * @param frame - the current frame to be delivered
+ */
+ protected void deliverAndDrawFrame(CvCameraViewFrame frame) {
+ Mat modified;
+
+ if (mListener != null) {
+ modified = mListener.onCameraFrame(frame);
+ } else {
+ modified = frame.rgba();
+ }
+
+ boolean bmpValid = true;
+ if (modified != null) {
+ try {
+ Utils.matToBitmap(modified, mCacheBitmap);
+ } catch(Exception e) {
+ Log.e(TAG, "Mat type: " + modified);
+ Log.e(TAG, "Bitmap type: " + mCacheBitmap.getWidth() + "*" + mCacheBitmap.getHeight());
+ Log.e(TAG, "Utils.matToBitmap() throws an exception: " + e.getMessage());
+ bmpValid = false;
+ }
+ }
+
+ if (bmpValid && mCacheBitmap != null) {
+ Canvas canvas = getHolder().lockCanvas();
+ if (canvas != null) {
+ canvas.drawColor(0, android.graphics.PorterDuff.Mode.CLEAR);
+ if (BuildConfig.DEBUG)
+ Log.d(TAG, "mStretch value: " + mScale);
+
+ if (mScale != 0) {
+ canvas.drawBitmap(mCacheBitmap, new Rect(0,0,mCacheBitmap.getWidth(), mCacheBitmap.getHeight()),
+ new Rect((int)((canvas.getWidth() - mScale*mCacheBitmap.getWidth()) / 2),
+ (int)((canvas.getHeight() - mScale*mCacheBitmap.getHeight()) / 2),
+ (int)((canvas.getWidth() - mScale*mCacheBitmap.getWidth()) / 2 + mScale*mCacheBitmap.getWidth()),
+ (int)((canvas.getHeight() - mScale*mCacheBitmap.getHeight()) / 2 + mScale*mCacheBitmap.getHeight())), null);
+ } else {
+ canvas.drawBitmap(mCacheBitmap, new Rect(0,0,mCacheBitmap.getWidth(), mCacheBitmap.getHeight()),
+ new Rect((canvas.getWidth() - mCacheBitmap.getWidth()) / 2,
+ (canvas.getHeight() - mCacheBitmap.getHeight()) / 2,
+ (canvas.getWidth() - mCacheBitmap.getWidth()) / 2 + mCacheBitmap.getWidth(),
+ (canvas.getHeight() - mCacheBitmap.getHeight()) / 2 + mCacheBitmap.getHeight()), null);
+ }
+
+ if (mFpsMeter != null) {
+ mFpsMeter.measure();
+ mFpsMeter.draw(canvas, 20, 30);
+ }
+ getHolder().unlockCanvasAndPost(canvas);
+ }
+ }
+ }
+
+ /**
+ * This method is invoked shall perform concrete operation to initialize the camera.
+ * CONTRACT: as a result of this method variables mFrameWidth and mFrameHeight MUST be
+ * initialized with the size of the Camera frames that will be delivered to external processor.
+ * @param width - the width of this SurfaceView
+ * @param height - the height of this SurfaceView
+ */
+ protected abstract boolean connectCamera(int width, int height);
+
+ /**
+ * Disconnects and release the particular camera object being connected to this surface view.
+ * Called when syncObject lock is held
+ */
+ protected abstract void disconnectCamera();
+
+ // NOTE: On Android 4.1.x the function must be called before SurfaceTexture constructor!
+ protected void AllocateCache()
+ {
+ mCacheBitmap = Bitmap.createBitmap(mFrameWidth, mFrameHeight, Bitmap.Config.ARGB_8888);
+ }
+
+ public interface ListItemAccessor {
+ public int getWidth(Object obj);
+ public int getHeight(Object obj);
+ };
+
+ /**
+ * This helper method can be called by subclasses to select camera preview size.
+ * It goes over the list of the supported preview sizes and selects the maximum one which
+ * fits both values set via setMaxFrameSize() and surface frame allocated for this view
+ * @param supportedSizes
+ * @param surfaceWidth
+ * @param surfaceHeight
+ * @return optimal frame size
+ */
+ protected Size calculateCameraFrameSize(List> supportedSizes, ListItemAccessor accessor, int surfaceWidth, int surfaceHeight) {
+ int calcWidth = 0;
+ int calcHeight = 0;
+
+ int maxAllowedWidth = (mMaxWidth != MAX_UNSPECIFIED && mMaxWidth < surfaceWidth)? mMaxWidth : surfaceWidth;
+ int maxAllowedHeight = (mMaxHeight != MAX_UNSPECIFIED && mMaxHeight < surfaceHeight)? mMaxHeight : surfaceHeight;
+
+ for (Object size : supportedSizes) {
+ int width = accessor.getWidth(size);
+ int height = accessor.getHeight(size);
+
+ if (width <= maxAllowedWidth && height <= maxAllowedHeight) {
+ if (width >= calcWidth && height >= calcHeight) {
+ calcWidth = (int) width;
+ calcHeight = (int) height;
+ }
+ }
+ }
+
+ return new Size(calcWidth, calcHeight);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/CameraGLRendererBase.java b/openCVLibrary330/src/main/java/org/opencv/android/CameraGLRendererBase.java
new file mode 100644
index 00000000000..60c37c304e2
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/CameraGLRendererBase.java
@@ -0,0 +1,440 @@
+package org.opencv.android;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.FloatBuffer;
+
+import javax.microedition.khronos.egl.EGLConfig;
+import javax.microedition.khronos.opengles.GL10;
+
+import org.opencv.android.CameraGLSurfaceView.CameraTextureListener;
+
+import android.annotation.TargetApi;
+import android.graphics.SurfaceTexture;
+import android.opengl.GLES11Ext;
+import android.opengl.GLES20;
+import android.opengl.GLSurfaceView;
+import android.util.Log;
+import android.view.View;
+
+@TargetApi(15)
+public abstract class CameraGLRendererBase implements GLSurfaceView.Renderer, SurfaceTexture.OnFrameAvailableListener {
+
+ protected final String LOGTAG = "CameraGLRendererBase";
+
+ // shaders
+ private final String vss = ""
+ + "attribute vec2 vPosition;\n"
+ + "attribute vec2 vTexCoord;\n" + "varying vec2 texCoord;\n"
+ + "void main() {\n" + " texCoord = vTexCoord;\n"
+ + " gl_Position = vec4 ( vPosition.x, vPosition.y, 0.0, 1.0 );\n"
+ + "}";
+
+ private final String fssOES = ""
+ + "#extension GL_OES_EGL_image_external : require\n"
+ + "precision mediump float;\n"
+ + "uniform samplerExternalOES sTexture;\n"
+ + "varying vec2 texCoord;\n"
+ + "void main() {\n"
+ + " gl_FragColor = texture2D(sTexture,texCoord);\n" + "}";
+
+ private final String fss2D = ""
+ + "precision mediump float;\n"
+ + "uniform sampler2D sTexture;\n"
+ + "varying vec2 texCoord;\n"
+ + "void main() {\n"
+ + " gl_FragColor = texture2D(sTexture,texCoord);\n" + "}";
+
+ // coord-s
+ private final float vertices[] = {
+ -1, -1,
+ -1, 1,
+ 1, -1,
+ 1, 1 };
+ private final float texCoordOES[] = {
+ 0, 1,
+ 0, 0,
+ 1, 1,
+ 1, 0 };
+ private final float texCoord2D[] = {
+ 0, 0,
+ 0, 1,
+ 1, 0,
+ 1, 1 };
+
+ private int[] texCamera = {0}, texFBO = {0}, texDraw = {0};
+ private int[] FBO = {0};
+ private int progOES = -1, prog2D = -1;
+ private int vPosOES, vTCOES, vPos2D, vTC2D;
+
+ private FloatBuffer vert, texOES, tex2D;
+
+ protected int mCameraWidth = -1, mCameraHeight = -1;
+ protected int mFBOWidth = -1, mFBOHeight = -1;
+ protected int mMaxCameraWidth = -1, mMaxCameraHeight = -1;
+ protected int mCameraIndex = CameraBridgeViewBase.CAMERA_ID_ANY;
+
+ protected SurfaceTexture mSTexture;
+
+ protected boolean mHaveSurface = false;
+ protected boolean mHaveFBO = false;
+ protected boolean mUpdateST = false;
+ protected boolean mEnabled = true;
+ protected boolean mIsStarted = false;
+
+ protected CameraGLSurfaceView mView;
+
+ protected abstract void openCamera(int id);
+ protected abstract void closeCamera();
+ protected abstract void setCameraPreviewSize(int width, int height); // updates mCameraWidth & mCameraHeight
+
+ public CameraGLRendererBase(CameraGLSurfaceView view) {
+ mView = view;
+ int bytes = vertices.length * Float.SIZE / Byte.SIZE;
+ vert = ByteBuffer.allocateDirect(bytes).order(ByteOrder.nativeOrder()).asFloatBuffer();
+ texOES = ByteBuffer.allocateDirect(bytes).order(ByteOrder.nativeOrder()).asFloatBuffer();
+ tex2D = ByteBuffer.allocateDirect(bytes).order(ByteOrder.nativeOrder()).asFloatBuffer();
+ vert.put(vertices).position(0);
+ texOES.put(texCoordOES).position(0);
+ tex2D.put(texCoord2D).position(0);
+ }
+
+ @Override
+ public synchronized void onFrameAvailable(SurfaceTexture surfaceTexture) {
+ //Log.i(LOGTAG, "onFrameAvailable");
+ mUpdateST = true;
+ mView.requestRender();
+ }
+
+ @Override
+ public void onDrawFrame(GL10 gl) {
+ //Log.i(LOGTAG, "onDrawFrame start");
+
+ if (!mHaveFBO)
+ return;
+
+ synchronized(this) {
+ if (mUpdateST) {
+ mSTexture.updateTexImage();
+ mUpdateST = false;
+ }
+
+ GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT);
+
+ CameraTextureListener texListener = mView.getCameraTextureListener();
+ if(texListener != null) {
+ //Log.d(LOGTAG, "haveUserCallback");
+ // texCamera(OES) -> texFBO
+ drawTex(texCamera[0], true, FBO[0]);
+
+ // call user code (texFBO -> texDraw)
+ boolean modified = texListener.onCameraTexture(texFBO[0], texDraw[0], mCameraWidth, mCameraHeight);
+
+ if(modified) {
+ // texDraw -> screen
+ drawTex(texDraw[0], false, 0);
+ } else {
+ // texFBO -> screen
+ drawTex(texFBO[0], false, 0);
+ }
+ } else {
+ Log.d(LOGTAG, "texCamera(OES) -> screen");
+ // texCamera(OES) -> screen
+ drawTex(texCamera[0], true, 0);
+ }
+ //Log.i(LOGTAG, "onDrawFrame end");
+ }
+ }
+
+ @Override
+ public void onSurfaceChanged(GL10 gl, int surfaceWidth, int surfaceHeight) {
+ Log.i(LOGTAG, "onSurfaceChanged("+surfaceWidth+"x"+surfaceHeight+")");
+ mHaveSurface = true;
+ updateState();
+ setPreviewSize(surfaceWidth, surfaceHeight);
+ }
+
+ @Override
+ public void onSurfaceCreated(GL10 gl, EGLConfig config) {
+ Log.i(LOGTAG, "onSurfaceCreated");
+ initShaders();
+ }
+
+ private void initShaders() {
+ String strGLVersion = GLES20.glGetString(GLES20.GL_VERSION);
+ if (strGLVersion != null)
+ Log.i(LOGTAG, "OpenGL ES version: " + strGLVersion);
+
+ GLES20.glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
+
+ progOES = loadShader(vss, fssOES);
+ vPosOES = GLES20.glGetAttribLocation(progOES, "vPosition");
+ vTCOES = GLES20.glGetAttribLocation(progOES, "vTexCoord");
+ GLES20.glEnableVertexAttribArray(vPosOES);
+ GLES20.glEnableVertexAttribArray(vTCOES);
+
+ prog2D = loadShader(vss, fss2D);
+ vPos2D = GLES20.glGetAttribLocation(prog2D, "vPosition");
+ vTC2D = GLES20.glGetAttribLocation(prog2D, "vTexCoord");
+ GLES20.glEnableVertexAttribArray(vPos2D);
+ GLES20.glEnableVertexAttribArray(vTC2D);
+ }
+
+ private void initSurfaceTexture() {
+ Log.d(LOGTAG, "initSurfaceTexture");
+ deleteSurfaceTexture();
+ initTexOES(texCamera);
+ mSTexture = new SurfaceTexture(texCamera[0]);
+ mSTexture.setOnFrameAvailableListener(this);
+ }
+
+ private void deleteSurfaceTexture() {
+ Log.d(LOGTAG, "deleteSurfaceTexture");
+ if(mSTexture != null) {
+ mSTexture.release();
+ mSTexture = null;
+ deleteTex(texCamera);
+ }
+ }
+
+ private void initTexOES(int[] tex) {
+ if(tex.length == 1) {
+ GLES20.glGenTextures(1, tex, 0);
+ GLES20.glBindTexture(GLES11Ext.GL_TEXTURE_EXTERNAL_OES, tex[0]);
+ GLES20.glTexParameteri(GLES11Ext.GL_TEXTURE_EXTERNAL_OES, GLES20.GL_TEXTURE_WRAP_S, GLES20.GL_CLAMP_TO_EDGE);
+ GLES20.glTexParameteri(GLES11Ext.GL_TEXTURE_EXTERNAL_OES, GLES20.GL_TEXTURE_WRAP_T, GLES20.GL_CLAMP_TO_EDGE);
+ GLES20.glTexParameteri(GLES11Ext.GL_TEXTURE_EXTERNAL_OES, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_NEAREST);
+ GLES20.glTexParameteri(GLES11Ext.GL_TEXTURE_EXTERNAL_OES, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_NEAREST);
+ }
+ }
+
+ private static void deleteTex(int[] tex) {
+ if(tex.length == 1) {
+ GLES20.glDeleteTextures(1, tex, 0);
+ }
+ }
+
+ private static int loadShader(String vss, String fss) {
+ Log.d("CameraGLRendererBase", "loadShader");
+ int vshader = GLES20.glCreateShader(GLES20.GL_VERTEX_SHADER);
+ GLES20.glShaderSource(vshader, vss);
+ GLES20.glCompileShader(vshader);
+ int[] status = new int[1];
+ GLES20.glGetShaderiv(vshader, GLES20.GL_COMPILE_STATUS, status, 0);
+ if (status[0] == 0) {
+ Log.e("CameraGLRendererBase", "Could not compile vertex shader: "+GLES20.glGetShaderInfoLog(vshader));
+ GLES20.glDeleteShader(vshader);
+ vshader = 0;
+ return 0;
+ }
+
+ int fshader = GLES20.glCreateShader(GLES20.GL_FRAGMENT_SHADER);
+ GLES20.glShaderSource(fshader, fss);
+ GLES20.glCompileShader(fshader);
+ GLES20.glGetShaderiv(fshader, GLES20.GL_COMPILE_STATUS, status, 0);
+ if (status[0] == 0) {
+ Log.e("CameraGLRendererBase", "Could not compile fragment shader:"+GLES20.glGetShaderInfoLog(fshader));
+ GLES20.glDeleteShader(vshader);
+ GLES20.glDeleteShader(fshader);
+ fshader = 0;
+ return 0;
+ }
+
+ int program = GLES20.glCreateProgram();
+ GLES20.glAttachShader(program, vshader);
+ GLES20.glAttachShader(program, fshader);
+ GLES20.glLinkProgram(program);
+ GLES20.glDeleteShader(vshader);
+ GLES20.glDeleteShader(fshader);
+ GLES20.glGetProgramiv(program, GLES20.GL_LINK_STATUS, status, 0);
+ if (status[0] == 0) {
+ Log.e("CameraGLRendererBase", "Could not link shader program: "+GLES20.glGetProgramInfoLog(program));
+ program = 0;
+ return 0;
+ }
+ GLES20.glValidateProgram(program);
+ GLES20.glGetProgramiv(program, GLES20.GL_VALIDATE_STATUS, status, 0);
+ if (status[0] == 0)
+ {
+ Log.e("CameraGLRendererBase", "Shader program validation error: "+GLES20.glGetProgramInfoLog(program));
+ GLES20.glDeleteProgram(program);
+ program = 0;
+ return 0;
+ }
+
+ Log.d("CameraGLRendererBase", "Shader program is built OK");
+
+ return program;
+ }
+
+ private void deleteFBO()
+ {
+ Log.d(LOGTAG, "deleteFBO("+mFBOWidth+"x"+mFBOHeight+")");
+ GLES20.glBindFramebuffer(GLES20.GL_FRAMEBUFFER, 0);
+ GLES20.glDeleteFramebuffers(1, FBO, 0);
+
+ deleteTex(texFBO);
+ deleteTex(texDraw);
+ mFBOWidth = mFBOHeight = 0;
+ }
+
+ private void initFBO(int width, int height)
+ {
+ Log.d(LOGTAG, "initFBO("+width+"x"+height+")");
+
+ deleteFBO();
+
+ GLES20.glGenTextures(1, texDraw, 0);
+ GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, texDraw[0]);
+ GLES20.glTexImage2D(GLES20.GL_TEXTURE_2D, 0, GLES20.GL_RGBA, width, height, 0, GLES20.GL_RGBA, GLES20.GL_UNSIGNED_BYTE, null);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_WRAP_S, GLES20.GL_CLAMP_TO_EDGE);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_WRAP_T, GLES20.GL_CLAMP_TO_EDGE);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_NEAREST);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_NEAREST);
+
+ GLES20.glGenTextures(1, texFBO, 0);
+ GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, texFBO[0]);
+ GLES20.glTexImage2D(GLES20.GL_TEXTURE_2D, 0, GLES20.GL_RGBA, width, height, 0, GLES20.GL_RGBA, GLES20.GL_UNSIGNED_BYTE, null);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_WRAP_S, GLES20.GL_CLAMP_TO_EDGE);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_WRAP_T, GLES20.GL_CLAMP_TO_EDGE);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_NEAREST);
+ GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_NEAREST);
+
+ //int hFBO;
+ GLES20.glGenFramebuffers(1, FBO, 0);
+ GLES20.glBindFramebuffer(GLES20.GL_FRAMEBUFFER, FBO[0]);
+ GLES20.glFramebufferTexture2D(GLES20.GL_FRAMEBUFFER, GLES20.GL_COLOR_ATTACHMENT0, GLES20.GL_TEXTURE_2D, texFBO[0], 0);
+ Log.d(LOGTAG, "initFBO error status: " + GLES20.glGetError());
+
+ int FBOstatus = GLES20.glCheckFramebufferStatus(GLES20.GL_FRAMEBUFFER);
+ if (FBOstatus != GLES20.GL_FRAMEBUFFER_COMPLETE)
+ Log.e(LOGTAG, "initFBO failed, status: " + FBOstatus);
+
+ mFBOWidth = width;
+ mFBOHeight = height;
+ }
+
+ // draw texture to FBO or to screen if fbo == 0
+ private void drawTex(int tex, boolean isOES, int fbo)
+ {
+ GLES20.glBindFramebuffer(GLES20.GL_FRAMEBUFFER, fbo);
+
+ if(fbo == 0)
+ GLES20.glViewport(0, 0, mView.getWidth(), mView.getHeight());
+ else
+ GLES20.glViewport(0, 0, mFBOWidth, mFBOHeight);
+
+ GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT);
+
+ if(isOES) {
+ GLES20.glUseProgram(progOES);
+ GLES20.glVertexAttribPointer(vPosOES, 2, GLES20.GL_FLOAT, false, 4*2, vert);
+ GLES20.glVertexAttribPointer(vTCOES, 2, GLES20.GL_FLOAT, false, 4*2, texOES);
+ } else {
+ GLES20.glUseProgram(prog2D);
+ GLES20.glVertexAttribPointer(vPos2D, 2, GLES20.GL_FLOAT, false, 4*2, vert);
+ GLES20.glVertexAttribPointer(vTC2D, 2, GLES20.GL_FLOAT, false, 4*2, tex2D);
+ }
+
+ GLES20.glActiveTexture(GLES20.GL_TEXTURE0);
+
+ if(isOES) {
+ GLES20.glBindTexture(GLES11Ext.GL_TEXTURE_EXTERNAL_OES, tex);
+ GLES20.glUniform1i(GLES20.glGetUniformLocation(progOES, "sTexture"), 0);
+ } else {
+ GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, tex);
+ GLES20.glUniform1i(GLES20.glGetUniformLocation(prog2D, "sTexture"), 0);
+ }
+
+ GLES20.glDrawArrays(GLES20.GL_TRIANGLE_STRIP, 0, 4);
+ GLES20.glFlush();
+ }
+
+ public synchronized void enableView() {
+ Log.d(LOGTAG, "enableView");
+ mEnabled = true;
+ updateState();
+ }
+
+ public synchronized void disableView() {
+ Log.d(LOGTAG, "disableView");
+ mEnabled = false;
+ updateState();
+ }
+
+ protected void updateState() {
+ Log.d(LOGTAG, "updateState");
+ Log.d(LOGTAG, "mEnabled="+mEnabled+", mHaveSurface="+mHaveSurface);
+ boolean willStart = mEnabled && mHaveSurface && mView.getVisibility() == View.VISIBLE;
+ if (willStart != mIsStarted) {
+ if(willStart) doStart();
+ else doStop();
+ } else {
+ Log.d(LOGTAG, "keeping State unchanged");
+ }
+ Log.d(LOGTAG, "updateState end");
+ }
+
+ protected synchronized void doStart() {
+ Log.d(LOGTAG, "doStart");
+ initSurfaceTexture();
+ openCamera(mCameraIndex);
+ mIsStarted = true;
+ if(mCameraWidth>0 && mCameraHeight>0)
+ setPreviewSize(mCameraWidth, mCameraHeight); // start preview and call listener.onCameraViewStarted()
+ }
+
+
+ protected void doStop() {
+ Log.d(LOGTAG, "doStop");
+ synchronized(this) {
+ mUpdateST = false;
+ mIsStarted = false;
+ mHaveFBO = false;
+ closeCamera();
+ deleteSurfaceTexture();
+ }
+ CameraTextureListener listener = mView.getCameraTextureListener();
+ if(listener != null) listener.onCameraViewStopped();
+
+ }
+
+ protected void setPreviewSize(int width, int height) {
+ synchronized(this) {
+ mHaveFBO = false;
+ mCameraWidth = width;
+ mCameraHeight = height;
+ setCameraPreviewSize(width, height); // can change mCameraWidth & mCameraHeight
+ initFBO(mCameraWidth, mCameraHeight);
+ mHaveFBO = true;
+ }
+
+ CameraTextureListener listener = mView.getCameraTextureListener();
+ if(listener != null) listener.onCameraViewStarted(mCameraWidth, mCameraHeight);
+ }
+
+ public void setCameraIndex(int cameraIndex) {
+ disableView();
+ mCameraIndex = cameraIndex;
+ enableView();
+ }
+
+ public void setMaxCameraPreviewSize(int maxWidth, int maxHeight) {
+ disableView();
+ mMaxCameraWidth = maxWidth;
+ mMaxCameraHeight = maxHeight;
+ enableView();
+ }
+
+ public void onResume() {
+ Log.i(LOGTAG, "onResume");
+ }
+
+ public void onPause() {
+ Log.i(LOGTAG, "onPause");
+ mHaveSurface = false;
+ updateState();
+ mCameraWidth = mCameraHeight = -1;
+ }
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/CameraGLSurfaceView.java b/openCVLibrary330/src/main/java/org/opencv/android/CameraGLSurfaceView.java
new file mode 100644
index 00000000000..05f950b4713
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/CameraGLSurfaceView.java
@@ -0,0 +1,119 @@
+package org.opencv.android;
+
+import org.opencv.R;
+
+import android.content.Context;
+import android.content.res.TypedArray;
+import android.opengl.GLSurfaceView;
+import android.util.AttributeSet;
+import android.util.Log;
+import android.view.SurfaceHolder;
+
+public class CameraGLSurfaceView extends GLSurfaceView {
+
+ private static final String LOGTAG = "CameraGLSurfaceView";
+
+ public interface CameraTextureListener {
+ /**
+ * This method is invoked when camera preview has started. After this method is invoked
+ * the frames will start to be delivered to client via the onCameraFrame() callback.
+ * @param width - the width of the frames that will be delivered
+ * @param height - the height of the frames that will be delivered
+ */
+ public void onCameraViewStarted(int width, int height);
+
+ /**
+ * This method is invoked when camera preview has been stopped for some reason.
+ * No frames will be delivered via onCameraFrame() callback after this method is called.
+ */
+ public void onCameraViewStopped();
+
+ /**
+ * This method is invoked when a new preview frame from Camera is ready.
+ * @param texIn - the OpenGL texture ID that contains frame in RGBA format
+ * @param texOut - the OpenGL texture ID that can be used to store modified frame image t display
+ * @param width - the width of the frame
+ * @param height - the height of the frame
+ * @return `true` if `texOut` should be displayed, `false` - to show `texIn`
+ */
+ public boolean onCameraTexture(int texIn, int texOut, int width, int height);
+ };
+
+ private CameraTextureListener mTexListener;
+ private CameraGLRendererBase mRenderer;
+
+ public CameraGLSurfaceView(Context context, AttributeSet attrs) {
+ super(context, attrs);
+
+ TypedArray styledAttrs = getContext().obtainStyledAttributes(attrs, R.styleable.CameraBridgeViewBase);
+ int cameraIndex = styledAttrs.getInt(R.styleable.CameraBridgeViewBase_camera_id, -1);
+ styledAttrs.recycle();
+
+ if(android.os.Build.VERSION.SDK_INT >= 21)
+ mRenderer = new Camera2Renderer(this);
+ else
+ mRenderer = new CameraRenderer(this);
+
+ setCameraIndex(cameraIndex);
+
+ setEGLContextClientVersion(2);
+ setRenderer(mRenderer);
+ setRenderMode(GLSurfaceView.RENDERMODE_WHEN_DIRTY);
+ }
+
+ public void setCameraTextureListener(CameraTextureListener texListener)
+ {
+ mTexListener = texListener;
+ }
+
+ public CameraTextureListener getCameraTextureListener()
+ {
+ return mTexListener;
+ }
+
+ public void setCameraIndex(int cameraIndex) {
+ mRenderer.setCameraIndex(cameraIndex);
+ }
+
+ public void setMaxCameraPreviewSize(int maxWidth, int maxHeight) {
+ mRenderer.setMaxCameraPreviewSize(maxWidth, maxHeight);
+ }
+
+ @Override
+ public void surfaceCreated(SurfaceHolder holder) {
+ super.surfaceCreated(holder);
+ }
+
+ @Override
+ public void surfaceDestroyed(SurfaceHolder holder) {
+ mRenderer.mHaveSurface = false;
+ super.surfaceDestroyed(holder);
+ }
+
+ @Override
+ public void surfaceChanged(SurfaceHolder holder, int format, int w, int h) {
+ super.surfaceChanged(holder, format, w, h);
+ }
+
+ @Override
+ public void onResume() {
+ Log.i(LOGTAG, "onResume");
+ super.onResume();
+ mRenderer.onResume();
+ }
+
+ @Override
+ public void onPause() {
+ Log.i(LOGTAG, "onPause");
+ mRenderer.onPause();
+ super.onPause();
+ }
+
+ public void enableView() {
+ mRenderer.enableView();
+ }
+
+ public void disableView() {
+ mRenderer.disableView();
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/CameraRenderer.java b/openCVLibrary330/src/main/java/org/opencv/android/CameraRenderer.java
new file mode 100644
index 00000000000..2d668ffa6e6
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/CameraRenderer.java
@@ -0,0 +1,166 @@
+package org.opencv.android;
+
+import java.io.IOException;
+import java.util.List;
+
+import android.annotation.TargetApi;
+import android.hardware.Camera;
+import android.hardware.Camera.Size;
+import android.os.Build;
+import android.util.Log;
+
+@TargetApi(15)
+@SuppressWarnings("deprecation")
+public class CameraRenderer extends CameraGLRendererBase {
+
+ public static final String LOGTAG = "CameraRenderer";
+
+ private Camera mCamera;
+ private boolean mPreviewStarted = false;
+
+ CameraRenderer(CameraGLSurfaceView view) {
+ super(view);
+ }
+
+ @Override
+ protected synchronized void closeCamera() {
+ Log.i(LOGTAG, "closeCamera");
+ if(mCamera != null) {
+ mCamera.stopPreview();
+ mPreviewStarted = false;
+ mCamera.release();
+ mCamera = null;
+ }
+ }
+
+ @Override
+ protected synchronized void openCamera(int id) {
+ Log.i(LOGTAG, "openCamera");
+ closeCamera();
+ if (id == CameraBridgeViewBase.CAMERA_ID_ANY) {
+ Log.d(LOGTAG, "Trying to open camera with old open()");
+ try {
+ mCamera = Camera.open();
+ }
+ catch (Exception e){
+ Log.e(LOGTAG, "Camera is not available (in use or does not exist): " + e.getLocalizedMessage());
+ }
+
+ if(mCamera == null && Build.VERSION.SDK_INT >= Build.VERSION_CODES.GINGERBREAD) {
+ boolean connected = false;
+ for (int camIdx = 0; camIdx < Camera.getNumberOfCameras(); ++camIdx) {
+ Log.d(LOGTAG, "Trying to open camera with new open(" + camIdx + ")");
+ try {
+ mCamera = Camera.open(camIdx);
+ connected = true;
+ } catch (RuntimeException e) {
+ Log.e(LOGTAG, "Camera #" + camIdx + "failed to open: " + e.getLocalizedMessage());
+ }
+ if (connected) break;
+ }
+ }
+ } else {
+ if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.GINGERBREAD) {
+ int localCameraIndex = mCameraIndex;
+ if (mCameraIndex == CameraBridgeViewBase.CAMERA_ID_BACK) {
+ Log.i(LOGTAG, "Trying to open BACK camera");
+ Camera.CameraInfo cameraInfo = new Camera.CameraInfo();
+ for (int camIdx = 0; camIdx < Camera.getNumberOfCameras(); ++camIdx) {
+ Camera.getCameraInfo( camIdx, cameraInfo );
+ if (cameraInfo.facing == Camera.CameraInfo.CAMERA_FACING_BACK) {
+ localCameraIndex = camIdx;
+ break;
+ }
+ }
+ } else if (mCameraIndex == CameraBridgeViewBase.CAMERA_ID_FRONT) {
+ Log.i(LOGTAG, "Trying to open FRONT camera");
+ Camera.CameraInfo cameraInfo = new Camera.CameraInfo();
+ for (int camIdx = 0; camIdx < Camera.getNumberOfCameras(); ++camIdx) {
+ Camera.getCameraInfo( camIdx, cameraInfo );
+ if (cameraInfo.facing == Camera.CameraInfo.CAMERA_FACING_FRONT) {
+ localCameraIndex = camIdx;
+ break;
+ }
+ }
+ }
+ if (localCameraIndex == CameraBridgeViewBase.CAMERA_ID_BACK) {
+ Log.e(LOGTAG, "Back camera not found!");
+ } else if (localCameraIndex == CameraBridgeViewBase.CAMERA_ID_FRONT) {
+ Log.e(LOGTAG, "Front camera not found!");
+ } else {
+ Log.d(LOGTAG, "Trying to open camera with new open(" + localCameraIndex + ")");
+ try {
+ mCamera = Camera.open(localCameraIndex);
+ } catch (RuntimeException e) {
+ Log.e(LOGTAG, "Camera #" + localCameraIndex + "failed to open: " + e.getLocalizedMessage());
+ }
+ }
+ }
+ }
+ if(mCamera == null) {
+ Log.e(LOGTAG, "Error: can't open camera");
+ return;
+ }
+ Camera.Parameters params = mCamera.getParameters();
+ List FocusModes = params.getSupportedFocusModes();
+ if (FocusModes != null && FocusModes.contains(Camera.Parameters.FOCUS_MODE_CONTINUOUS_VIDEO))
+ {
+ params.setFocusMode(Camera.Parameters.FOCUS_MODE_CONTINUOUS_VIDEO);
+ }
+ mCamera.setParameters(params);
+
+ try {
+ mCamera.setPreviewTexture(mSTexture);
+ } catch (IOException ioe) {
+ Log.e(LOGTAG, "setPreviewTexture() failed: " + ioe.getMessage());
+ }
+ }
+
+ @Override
+ public synchronized void setCameraPreviewSize(int width, int height) {
+ Log.i(LOGTAG, "setCameraPreviewSize: "+width+"x"+height);
+ if(mCamera == null) {
+ Log.e(LOGTAG, "Camera isn't initialized!");
+ return;
+ }
+
+ if(mMaxCameraWidth > 0 && mMaxCameraWidth < width) width = mMaxCameraWidth;
+ if(mMaxCameraHeight > 0 && mMaxCameraHeight < height) height = mMaxCameraHeight;
+
+ Camera.Parameters param = mCamera.getParameters();
+ List psize = param.getSupportedPreviewSizes();
+ int bestWidth = 0, bestHeight = 0;
+ if (psize.size() > 0) {
+ float aspect = (float)width / height;
+ for (Size size : psize) {
+ int w = size.width, h = size.height;
+ Log.d(LOGTAG, "checking camera preview size: "+w+"x"+h);
+ if ( w <= width && h <= height &&
+ w >= bestWidth && h >= bestHeight &&
+ Math.abs(aspect - (float)w/h) < 0.2 ) {
+ bestWidth = w;
+ bestHeight = h;
+ }
+ }
+ if(bestWidth <= 0 || bestHeight <= 0) {
+ bestWidth = psize.get(0).width;
+ bestHeight = psize.get(0).height;
+ Log.e(LOGTAG, "Error: best size was not selected, using "+bestWidth+" x "+bestHeight);
+ } else {
+ Log.i(LOGTAG, "Selected best size: "+bestWidth+" x "+bestHeight);
+ }
+
+ if(mPreviewStarted) {
+ mCamera.stopPreview();
+ mPreviewStarted = false;
+ }
+ mCameraWidth = bestWidth;
+ mCameraHeight = bestHeight;
+ param.setPreviewSize(bestWidth, bestHeight);
+ }
+ param.set("orientation", "landscape");
+ mCamera.setParameters(param);
+ mCamera.startPreview();
+ mPreviewStarted = true;
+ }
+}
\ No newline at end of file
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/FpsMeter.java b/openCVLibrary330/src/main/java/org/opencv/android/FpsMeter.java
new file mode 100644
index 00000000000..88e826cf965
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/FpsMeter.java
@@ -0,0 +1,66 @@
+package org.opencv.android;
+
+import java.text.DecimalFormat;
+
+import org.opencv.core.Core;
+
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.util.Log;
+
+public class FpsMeter {
+ private static final String TAG = "FpsMeter";
+ private static final int STEP = 20;
+ private static final DecimalFormat FPS_FORMAT = new DecimalFormat("0.00");
+
+ private int mFramesCouner;
+ private double mFrequency;
+ private long mprevFrameTime;
+ private String mStrfps;
+ Paint mPaint;
+ boolean mIsInitialized = false;
+ int mWidth = 0;
+ int mHeight = 0;
+
+ public void init() {
+ mFramesCouner = 0;
+ mFrequency = Core.getTickFrequency();
+ mprevFrameTime = Core.getTickCount();
+ mStrfps = "";
+
+ mPaint = new Paint();
+ mPaint.setColor(Color.BLUE);
+ mPaint.setTextSize(20);
+ }
+
+ public void measure() {
+ if (!mIsInitialized) {
+ init();
+ mIsInitialized = true;
+ } else {
+ mFramesCouner++;
+ if (mFramesCouner % STEP == 0) {
+ long time = Core.getTickCount();
+ double fps = STEP * mFrequency / (time - mprevFrameTime);
+ mprevFrameTime = time;
+ if (mWidth != 0 && mHeight != 0)
+ mStrfps = FPS_FORMAT.format(fps) + " FPS@" + Integer.valueOf(mWidth) + "x" + Integer.valueOf(mHeight);
+ else
+ mStrfps = FPS_FORMAT.format(fps) + " FPS";
+ Log.i(TAG, mStrfps);
+ }
+ }
+ }
+
+ public void setResolution(int width, int height) {
+ mWidth = width;
+ mHeight = height;
+ }
+
+ public void draw(Canvas canvas, float offsetx, float offsety) {
+ Log.d(TAG, mStrfps);
+ canvas.drawText(mStrfps, offsetx, offsety, mPaint);
+ }
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/InstallCallbackInterface.java b/openCVLibrary330/src/main/java/org/opencv/android/InstallCallbackInterface.java
new file mode 100644
index 00000000000..f68027a7ba2
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/InstallCallbackInterface.java
@@ -0,0 +1,34 @@
+package org.opencv.android;
+
+/**
+ * Installation callback interface.
+ */
+public interface InstallCallbackInterface
+{
+ /**
+ * New package installation is required.
+ */
+ static final int NEW_INSTALLATION = 0;
+ /**
+ * Current package installation is in progress.
+ */
+ static final int INSTALLATION_PROGRESS = 1;
+
+ /**
+ * Target package name.
+ * @return Return target package name.
+ */
+ public String getPackageName();
+ /**
+ * Installation is approved.
+ */
+ public void install();
+ /**
+ * Installation is canceled.
+ */
+ public void cancel();
+ /**
+ * Wait for package installation.
+ */
+ public void wait_install();
+};
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/JavaCameraView.java b/openCVLibrary330/src/main/java/org/opencv/android/JavaCameraView.java
new file mode 100644
index 00000000000..c9b2e2dfc9a
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/JavaCameraView.java
@@ -0,0 +1,385 @@
+package org.opencv.android;
+
+import java.util.List;
+import java.util.logging.Logger;
+
+import android.content.Context;
+import android.graphics.ImageFormat;
+import android.graphics.SurfaceTexture;
+import android.hardware.Camera;
+import android.hardware.Camera.PreviewCallback;
+import android.os.Build;
+import android.util.AttributeSet;
+import android.util.Log;
+import android.view.ViewGroup.LayoutParams;
+
+import org.opencv.BuildConfig;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * This class is an implementation of the Bridge View between OpenCV and Java Camera.
+ * This class relays on the functionality available in base class and only implements
+ * required functions:
+ * connectCamera - opens Java camera and sets the PreviewCallback to be delivered.
+ * disconnectCamera - closes the camera and stops preview.
+ * When frame is delivered via callback from Camera - it processed via OpenCV to be
+ * converted to RGBA32 and then passed to the external callback for modifications if required.
+ */
+public class JavaCameraView extends CameraBridgeViewBase implements PreviewCallback {
+
+ private static final int MAGIC_TEXTURE_ID = 10;
+ private static final String TAG = "JavaCameraView";
+
+ private byte mBuffer[];
+ private Mat[] mFrameChain;
+ private int mChainIdx = 0;
+ private Thread mThread;
+ private boolean mStopThread;
+
+ protected Camera mCamera;
+ protected JavaCameraFrame[] mCameraFrame;
+ private SurfaceTexture mSurfaceTexture;
+ private int mPreviewFormat = ImageFormat.NV21;
+
+ public static class JavaCameraSizeAccessor implements ListItemAccessor {
+
+ @Override
+ public int getWidth(Object obj) {
+ Camera.Size size = (Camera.Size) obj;
+ return size.width;
+ }
+
+ @Override
+ public int getHeight(Object obj) {
+ Camera.Size size = (Camera.Size) obj;
+ return size.height;
+ }
+ }
+
+ public JavaCameraView(Context context, int cameraId) {
+ super(context, cameraId);
+ }
+
+ public JavaCameraView(Context context, AttributeSet attrs) {
+ super(context, attrs);
+ }
+
+ protected boolean initializeCamera(int width, int height) {
+ Log.d(TAG, "Initialize java camera");
+ boolean result = true;
+ synchronized (this) {
+ mCamera = null;
+
+ if (mCameraIndex == CAMERA_ID_ANY) {
+ Log.d(TAG, "Trying to open camera with old open()");
+ try {
+ mCamera = Camera.open();
+ }
+ catch (Exception e){
+ Log.e(TAG, "Camera is not available (in use or does not exist): " + e.getLocalizedMessage());
+ }
+
+ if(mCamera == null && Build.VERSION.SDK_INT >= Build.VERSION_CODES.GINGERBREAD) {
+ boolean connected = false;
+ for (int camIdx = 0; camIdx < Camera.getNumberOfCameras(); ++camIdx) {
+ Log.d(TAG, "Trying to open camera with new open(" + Integer.valueOf(camIdx) + ")");
+ try {
+ mCamera = Camera.open(camIdx);
+ connected = true;
+ } catch (RuntimeException e) {
+ Log.e(TAG, "Camera #" + camIdx + "failed to open: " + e.getLocalizedMessage());
+ }
+ if (connected) break;
+ }
+ }
+ } else {
+ if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.GINGERBREAD) {
+ int localCameraIndex = mCameraIndex;
+ if (mCameraIndex == CAMERA_ID_BACK) {
+ Log.i(TAG, "Trying to open back camera");
+ Camera.CameraInfo cameraInfo = new Camera.CameraInfo();
+ for (int camIdx = 0; camIdx < Camera.getNumberOfCameras(); ++camIdx) {
+ Camera.getCameraInfo( camIdx, cameraInfo );
+ if (cameraInfo.facing == Camera.CameraInfo.CAMERA_FACING_BACK) {
+ localCameraIndex = camIdx;
+ break;
+ }
+ }
+ } else if (mCameraIndex == CAMERA_ID_FRONT) {
+ Log.i(TAG, "Trying to open front camera");
+ Camera.CameraInfo cameraInfo = new Camera.CameraInfo();
+ for (int camIdx = 0; camIdx < Camera.getNumberOfCameras(); ++camIdx) {
+ Camera.getCameraInfo( camIdx, cameraInfo );
+ if (cameraInfo.facing == Camera.CameraInfo.CAMERA_FACING_FRONT) {
+ localCameraIndex = camIdx;
+ break;
+ }
+ }
+ }
+ if (localCameraIndex == CAMERA_ID_BACK) {
+ Log.e(TAG, "Back camera not found!");
+ } else if (localCameraIndex == CAMERA_ID_FRONT) {
+ Log.e(TAG, "Front camera not found!");
+ } else {
+ Log.d(TAG, "Trying to open camera with new open(" + Integer.valueOf(localCameraIndex) + ")");
+ try {
+ mCamera = Camera.open(localCameraIndex);
+ } catch (RuntimeException e) {
+ Log.e(TAG, "Camera #" + localCameraIndex + "failed to open: " + e.getLocalizedMessage());
+ }
+ }
+ }
+ }
+
+ if (mCamera == null)
+ return false;
+
+ /* Now set camera parameters */
+ try {
+ /* Focal length debug logging for funsizes */
+ Camera.Parameters params = mCamera.getParameters();
+
+ Log.i("TEST", "Horizontal angle:" + Float.toString(params.getHorizontalViewAngle()));
+ Log.i("TEST", "Vertical angle:" + Float.toString(params.getVerticalViewAngle()));
+
+ Log.d(TAG, "getSupportedPreviewSizes()");
+ List sizes = params.getSupportedPreviewSizes();
+
+ if (sizes != null) {
+ /* Select the size that fits surface considering maximum size allowed */
+ Size frameSize = calculateCameraFrameSize(sizes, new JavaCameraSizeAccessor(), width, height);
+
+ /* Image format NV21 causes issues in the Android emulators */
+ if (Build.FINGERPRINT.startsWith("generic")
+ || Build.FINGERPRINT.startsWith("unknown")
+ || Build.MODEL.contains("google_sdk")
+ || Build.MODEL.contains("Emulator")
+ || Build.MODEL.contains("Android SDK built for x86")
+ || Build.MANUFACTURER.contains("Genymotion")
+ || (Build.BRAND.startsWith("generic") && Build.DEVICE.startsWith("generic"))
+ || "google_sdk".equals(Build.PRODUCT))
+ params.setPreviewFormat(ImageFormat.YV12); // "generic" or "android" = android emulator
+ else
+ params.setPreviewFormat(ImageFormat.NV21);
+
+ mPreviewFormat = params.getPreviewFormat();
+
+ Log.d(TAG, "Set preview size to " + Integer.valueOf((int)frameSize.width) + "x" + Integer.valueOf((int)frameSize.height));
+ params.setPreviewSize((int)frameSize.width, (int)frameSize.height);
+
+ if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.ICE_CREAM_SANDWICH && !android.os.Build.MODEL.equals("GT-I9100"))
+ params.setRecordingHint(true);
+
+ List FocusModes = params.getSupportedFocusModes();
+ if (FocusModes != null && FocusModes.contains(Camera.Parameters.FOCUS_MODE_CONTINUOUS_VIDEO))
+ {
+ params.setFocusMode(Camera.Parameters.FOCUS_MODE_CONTINUOUS_VIDEO);
+ }
+
+ mCamera.setParameters(params);
+ params = mCamera.getParameters();
+
+ mFrameWidth = params.getPreviewSize().width;
+ mFrameHeight = params.getPreviewSize().height;
+
+ if ((getLayoutParams().width == LayoutParams.MATCH_PARENT) && (getLayoutParams().height == LayoutParams.MATCH_PARENT))
+ mScale = Math.min(((float)height)/mFrameHeight, ((float)width)/mFrameWidth);
+ else
+ mScale = 0;
+
+ if (mFpsMeter != null) {
+ mFpsMeter.setResolution(mFrameWidth, mFrameHeight);
+ }
+
+ int size = mFrameWidth * mFrameHeight;
+ size = size * ImageFormat.getBitsPerPixel(params.getPreviewFormat()) / 8;
+ mBuffer = new byte[size];
+
+ mCamera.addCallbackBuffer(mBuffer);
+ mCamera.setPreviewCallbackWithBuffer(this);
+
+ mFrameChain = new Mat[2];
+ mFrameChain[0] = new Mat(mFrameHeight + (mFrameHeight/2), mFrameWidth, CvType.CV_8UC1);
+ mFrameChain[1] = new Mat(mFrameHeight + (mFrameHeight/2), mFrameWidth, CvType.CV_8UC1);
+
+ AllocateCache();
+
+ mCameraFrame = new JavaCameraFrame[2];
+ mCameraFrame[0] = new JavaCameraFrame(mFrameChain[0], mFrameWidth, mFrameHeight);
+ mCameraFrame[1] = new JavaCameraFrame(mFrameChain[1], mFrameWidth, mFrameHeight);
+
+ if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.HONEYCOMB) {
+ mSurfaceTexture = new SurfaceTexture(MAGIC_TEXTURE_ID);
+ mCamera.setPreviewTexture(mSurfaceTexture);
+ } else
+ mCamera.setPreviewDisplay(null);
+
+ /* Finally we are ready to start the preview */
+ Log.d(TAG, "startPreview");
+ mCamera.startPreview();
+ }
+ else
+ result = false;
+ } catch (Exception e) {
+ result = false;
+ e.printStackTrace();
+ }
+ }
+
+ return result;
+ }
+
+ protected void releaseCamera() {
+ synchronized (this) {
+ if (mCamera != null) {
+ mCamera.stopPreview();
+ mCamera.setPreviewCallback(null);
+
+ mCamera.release();
+ }
+ mCamera = null;
+ if (mFrameChain != null) {
+ mFrameChain[0].release();
+ mFrameChain[1].release();
+ }
+ if (mCameraFrame != null) {
+ mCameraFrame[0].release();
+ mCameraFrame[1].release();
+ }
+ }
+ }
+
+ private boolean mCameraFrameReady = false;
+
+ @Override
+ protected boolean connectCamera(int width, int height) {
+
+ /* 1. We need to instantiate camera
+ * 2. We need to start thread which will be getting frames
+ */
+ /* First step - initialize camera connection */
+ Log.d(TAG, "Connecting to camera");
+ if (!initializeCamera(width, height))
+ return false;
+
+ mCameraFrameReady = false;
+
+ /* now we can start update thread */
+ Log.d(TAG, "Starting processing thread");
+ mStopThread = false;
+ mThread = new Thread(new CameraWorker());
+ mThread.start();
+
+ return true;
+ }
+
+ @Override
+ protected void disconnectCamera() {
+ /* 1. We need to stop thread which updating the frames
+ * 2. Stop camera and release it
+ */
+ Log.d(TAG, "Disconnecting from camera");
+ try {
+ mStopThread = true;
+ Log.d(TAG, "Notify thread");
+ synchronized (this) {
+ this.notify();
+ }
+ Log.d(TAG, "Waiting for thread");
+ if (mThread != null)
+ mThread.join();
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ } finally {
+ mThread = null;
+ }
+
+ /* Now release camera */
+ releaseCamera();
+
+ mCameraFrameReady = false;
+ }
+
+ @Override
+ public void onPreviewFrame(byte[] frame, Camera arg1) {
+ if (BuildConfig.DEBUG)
+ Log.d(TAG, "Preview Frame received. Frame size: " + frame.length);
+ synchronized (this) {
+ mFrameChain[mChainIdx].put(0, 0, frame);
+ mCameraFrameReady = true;
+ this.notify();
+ }
+ if (mCamera != null)
+ mCamera.addCallbackBuffer(mBuffer);
+ }
+
+ private class JavaCameraFrame implements CvCameraViewFrame {
+ @Override
+ public Mat gray() {
+ return mYuvFrameData.submat(0, mHeight, 0, mWidth);
+ }
+
+ @Override
+ public Mat rgba() {
+ if (mPreviewFormat == ImageFormat.NV21)
+ Imgproc.cvtColor(mYuvFrameData, mRgba, Imgproc.COLOR_YUV2RGBA_NV21, 4);
+ else if (mPreviewFormat == ImageFormat.YV12)
+ Imgproc.cvtColor(mYuvFrameData, mRgba, Imgproc.COLOR_YUV2RGB_I420, 4); // COLOR_YUV2RGBA_YV12 produces inverted colors
+ else
+ throw new IllegalArgumentException("Preview Format can be NV21 or YV12");
+
+ return mRgba;
+ }
+
+ public JavaCameraFrame(Mat Yuv420sp, int width, int height) {
+ super();
+ mWidth = width;
+ mHeight = height;
+ mYuvFrameData = Yuv420sp;
+ mRgba = new Mat();
+ }
+
+ public void release() {
+ mRgba.release();
+ }
+
+ private Mat mYuvFrameData;
+ private Mat mRgba;
+ private int mWidth;
+ private int mHeight;
+ };
+
+ private class CameraWorker implements Runnable {
+
+ @Override
+ public void run() {
+ do {
+ boolean hasFrame = false;
+ synchronized (JavaCameraView.this) {
+ try {
+ while (!mCameraFrameReady && !mStopThread) {
+ JavaCameraView.this.wait();
+ }
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+ if (mCameraFrameReady)
+ {
+ mChainIdx = 1 - mChainIdx;
+ mCameraFrameReady = false;
+ hasFrame = true;
+ }
+ }
+
+ if (!mStopThread && hasFrame) {
+ if (!mFrameChain[1 - mChainIdx].empty())
+ deliverAndDrawFrame(mCameraFrame[1 - mChainIdx]);
+ }
+ } while (!mStopThread);
+ Log.d(TAG, "Finish processing thread");
+ }
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/LoaderCallbackInterface.java b/openCVLibrary330/src/main/java/org/opencv/android/LoaderCallbackInterface.java
new file mode 100644
index 00000000000..a941e8377b6
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/LoaderCallbackInterface.java
@@ -0,0 +1,40 @@
+package org.opencv.android;
+
+/**
+ * Interface for callback object in case of asynchronous initialization of OpenCV.
+ */
+public interface LoaderCallbackInterface
+{
+ /**
+ * OpenCV initialization finished successfully.
+ */
+ static final int SUCCESS = 0;
+ /**
+ * Google Play Market cannot be invoked.
+ */
+ static final int MARKET_ERROR = 2;
+ /**
+ * OpenCV library installation has been canceled by the user.
+ */
+ static final int INSTALL_CANCELED = 3;
+ /**
+ * This version of OpenCV Manager Service is incompatible with the app. Possibly, a service update is required.
+ */
+ static final int INCOMPATIBLE_MANAGER_VERSION = 4;
+ /**
+ * OpenCV library initialization has failed.
+ */
+ static final int INIT_FAILED = 0xff;
+
+ /**
+ * Callback method, called after OpenCV library initialization.
+ * @param status status of initialization (see initialization status constants).
+ */
+ public void onManagerConnected(int status);
+
+ /**
+ * Callback method, called in case the package installation is needed.
+ * @param callback answer object with approve and cancel methods and the package description.
+ */
+ public void onPackageInstall(final int operation, InstallCallbackInterface callback);
+};
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/OpenCVLoader.java b/openCVLibrary330/src/main/java/org/opencv/android/OpenCVLoader.java
new file mode 100644
index 00000000000..947b9b89746
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/OpenCVLoader.java
@@ -0,0 +1,127 @@
+package org.opencv.android;
+
+import android.content.Context;
+
+/**
+ * Helper class provides common initialization methods for OpenCV library.
+ */
+public class OpenCVLoader
+{
+ /**
+ * OpenCV Library version 2.4.2.
+ */
+ public static final String OPENCV_VERSION_2_4_2 = "2.4.2";
+
+ /**
+ * OpenCV Library version 2.4.3.
+ */
+ public static final String OPENCV_VERSION_2_4_3 = "2.4.3";
+
+ /**
+ * OpenCV Library version 2.4.4.
+ */
+ public static final String OPENCV_VERSION_2_4_4 = "2.4.4";
+
+ /**
+ * OpenCV Library version 2.4.5.
+ */
+ public static final String OPENCV_VERSION_2_4_5 = "2.4.5";
+
+ /**
+ * OpenCV Library version 2.4.6.
+ */
+ public static final String OPENCV_VERSION_2_4_6 = "2.4.6";
+
+ /**
+ * OpenCV Library version 2.4.7.
+ */
+ public static final String OPENCV_VERSION_2_4_7 = "2.4.7";
+
+ /**
+ * OpenCV Library version 2.4.8.
+ */
+ public static final String OPENCV_VERSION_2_4_8 = "2.4.8";
+
+ /**
+ * OpenCV Library version 2.4.9.
+ */
+ public static final String OPENCV_VERSION_2_4_9 = "2.4.9";
+
+ /**
+ * OpenCV Library version 2.4.10.
+ */
+ public static final String OPENCV_VERSION_2_4_10 = "2.4.10";
+
+ /**
+ * OpenCV Library version 2.4.11.
+ */
+ public static final String OPENCV_VERSION_2_4_11 = "2.4.11";
+
+ /**
+ * OpenCV Library version 2.4.12.
+ */
+ public static final String OPENCV_VERSION_2_4_12 = "2.4.12";
+
+ /**
+ * OpenCV Library version 2.4.13.
+ */
+ public static final String OPENCV_VERSION_2_4_13 = "2.4.13";
+
+ /**
+ * OpenCV Library version 3.0.0.
+ */
+ public static final String OPENCV_VERSION_3_0_0 = "3.0.0";
+
+ /**
+ * OpenCV Library version 3.1.0.
+ */
+ public static final String OPENCV_VERSION_3_1_0 = "3.1.0";
+
+ /**
+ * OpenCV Library version 3.2.0.
+ */
+ public static final String OPENCV_VERSION_3_2_0 = "3.2.0";
+
+ /**
+ * OpenCV Library version 3.3.0.
+ */
+ public static final String OPENCV_VERSION_3_3_0 = "3.3.0";
+
+ /**
+ * Current OpenCV Library version
+ */
+ public static final String OPENCV_VERSION = "3.3.0";
+
+
+ /**
+ * Loads and initializes OpenCV library from current application package. Roughly, it's an analog of system.loadLibrary("opencv_java").
+ * @return Returns true is initialization of OpenCV was successful.
+ */
+ public static boolean initDebug()
+ {
+ return StaticHelper.initOpenCV(false);
+ }
+
+ /**
+ * Loads and initializes OpenCV library from current application package. Roughly, it's an analog of system.loadLibrary("opencv_java").
+ * @param InitCuda load and initialize CUDA runtime libraries.
+ * @return Returns true is initialization of OpenCV was successful.
+ */
+ public static boolean initDebug(boolean InitCuda)
+ {
+ return StaticHelper.initOpenCV(InitCuda);
+ }
+
+ /**
+ * Loads and initializes OpenCV library using OpenCV Engine service.
+ * @param Version OpenCV library version.
+ * @param AppContext application context for connecting to the service.
+ * @param Callback object, that implements LoaderCallbackInterface for handling the connection status.
+ * @return Returns true if initialization of OpenCV is successful.
+ */
+ public static boolean initAsync(String Version, Context AppContext,
+ LoaderCallbackInterface Callback)
+ {
+ return AsyncServiceHelper.initOpenCV(Version, AppContext, Callback);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/StaticHelper.java b/openCVLibrary330/src/main/java/org/opencv/android/StaticHelper.java
new file mode 100644
index 00000000000..36f9f6f64a4
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/StaticHelper.java
@@ -0,0 +1,104 @@
+package org.opencv.android;
+
+import org.opencv.core.Core;
+
+import java.util.StringTokenizer;
+import android.util.Log;
+
+class StaticHelper {
+
+ public static boolean initOpenCV(boolean InitCuda)
+ {
+ boolean result;
+ String libs = "";
+
+ if(InitCuda)
+ {
+ loadLibrary("cudart");
+ loadLibrary("nppc");
+ loadLibrary("nppi");
+ loadLibrary("npps");
+ loadLibrary("cufft");
+ loadLibrary("cublas");
+ }
+
+ Log.d(TAG, "Trying to get library list");
+
+ try
+ {
+ System.loadLibrary("opencv_info");
+ libs = getLibraryList();
+ }
+ catch(UnsatisfiedLinkError e)
+ {
+ Log.e(TAG, "OpenCV error: Cannot load info library for OpenCV");
+ }
+
+ Log.d(TAG, "Library list: \"" + libs + "\"");
+ Log.d(TAG, "First attempt to load libs");
+ if (initOpenCVLibs(libs))
+ {
+ Log.d(TAG, "First attempt to load libs is OK");
+ String eol = System.getProperty("line.separator");
+ for (String str : Core.getBuildInformation().split(eol))
+ Log.i(TAG, str);
+
+ result = true;
+ }
+ else
+ {
+ Log.d(TAG, "First attempt to load libs fails");
+ result = false;
+ }
+
+ return result;
+ }
+
+ private static boolean loadLibrary(String Name)
+ {
+ boolean result = true;
+
+ Log.d(TAG, "Trying to load library " + Name);
+ try
+ {
+ System.loadLibrary(Name);
+ Log.d(TAG, "Library " + Name + " loaded");
+ }
+ catch(UnsatisfiedLinkError e)
+ {
+ Log.d(TAG, "Cannot load library \"" + Name + "\"");
+ e.printStackTrace();
+ result &= false;
+ }
+
+ return result;
+ }
+
+ private static boolean initOpenCVLibs(String Libs)
+ {
+ Log.d(TAG, "Trying to init OpenCV libs");
+
+ boolean result = true;
+
+ if ((null != Libs) && (Libs.length() != 0))
+ {
+ Log.d(TAG, "Trying to load libs by dependency list");
+ StringTokenizer splitter = new StringTokenizer(Libs, ";");
+ while(splitter.hasMoreTokens())
+ {
+ result &= loadLibrary(splitter.nextToken());
+ }
+ }
+ else
+ {
+ // If dependencies list is not defined or empty.
+ result &= loadLibrary("opencv_java3");
+ }
+
+ return result;
+ }
+
+ private static final String TAG = "OpenCV/StaticHelper";
+
+ private static native String getLibraryList();
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/android/Utils.java b/openCVLibrary330/src/main/java/org/opencv/android/Utils.java
new file mode 100644
index 00000000000..404c986da80
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/android/Utils.java
@@ -0,0 +1,139 @@
+package org.opencv.android;
+
+import android.content.Context;
+import android.graphics.Bitmap;
+
+import org.opencv.core.CvException;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
+
+import java.io.ByteArrayOutputStream;
+import java.io.File;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.io.InputStream;
+
+public class Utils {
+
+ public static String exportResource(Context context, int resourceId) {
+ return exportResource(context, resourceId, "OpenCV_data");
+ }
+
+ public static String exportResource(Context context, int resourceId, String dirname) {
+ String fullname = context.getResources().getString(resourceId);
+ String resName = fullname.substring(fullname.lastIndexOf("/") + 1);
+ try {
+ InputStream is = context.getResources().openRawResource(resourceId);
+ File resDir = context.getDir(dirname, Context.MODE_PRIVATE);
+ File resFile = new File(resDir, resName);
+
+ FileOutputStream os = new FileOutputStream(resFile);
+
+ byte[] buffer = new byte[4096];
+ int bytesRead;
+ while ((bytesRead = is.read(buffer)) != -1) {
+ os.write(buffer, 0, bytesRead);
+ }
+ is.close();
+ os.close();
+
+ return resFile.getAbsolutePath();
+ } catch (IOException e) {
+ e.printStackTrace();
+ throw new CvException("Failed to export resource " + resName
+ + ". Exception thrown: " + e);
+ }
+ }
+
+ public static Mat loadResource(Context context, int resourceId) throws IOException
+ {
+ return loadResource(context, resourceId, -1);
+ }
+
+ public static Mat loadResource(Context context, int resourceId, int flags) throws IOException
+ {
+ InputStream is = context.getResources().openRawResource(resourceId);
+ ByteArrayOutputStream os = new ByteArrayOutputStream(is.available());
+
+ byte[] buffer = new byte[4096];
+ int bytesRead;
+ while ((bytesRead = is.read(buffer)) != -1) {
+ os.write(buffer, 0, bytesRead);
+ }
+ is.close();
+
+ Mat encoded = new Mat(1, os.size(), CvType.CV_8U);
+ encoded.put(0, 0, os.toByteArray());
+ os.close();
+
+ Mat decoded = Imgcodecs.imdecode(encoded, flags);
+ encoded.release();
+
+ return decoded;
+ }
+
+ /**
+ * Converts Android Bitmap to OpenCV Mat.
+ *
+ * This function converts an Android Bitmap image to the OpenCV Mat.
+ *
'ARGB_8888' and 'RGB_565' input Bitmap formats are supported.
+ *
The output Mat is always created of the same size as the input Bitmap and of the 'CV_8UC4' type,
+ * it keeps the image in RGBA format.
+ *
This function throws an exception if the conversion fails.
+ * @param bmp is a valid input Bitmap object of the type 'ARGB_8888' or 'RGB_565'.
+ * @param mat is a valid output Mat object, it will be reallocated if needed, so it may be empty.
+ * @param unPremultiplyAlpha is a flag, that determines, whether the bitmap needs to be converted from alpha premultiplied format (like Android keeps 'ARGB_8888' ones) to regular one; this flag is ignored for 'RGB_565' bitmaps.
+ */
+ public static void bitmapToMat(Bitmap bmp, Mat mat, boolean unPremultiplyAlpha) {
+ if (bmp == null)
+ throw new java.lang.IllegalArgumentException("bmp == null");
+ if (mat == null)
+ throw new java.lang.IllegalArgumentException("mat == null");
+ nBitmapToMat2(bmp, mat.nativeObj, unPremultiplyAlpha);
+ }
+
+ /**
+ * Short form of the bitmapToMat(bmp, mat, unPremultiplyAlpha=false).
+ * @param bmp is a valid input Bitmap object of the type 'ARGB_8888' or 'RGB_565'.
+ * @param mat is a valid output Mat object, it will be reallocated if needed, so Mat may be empty.
+ */
+ public static void bitmapToMat(Bitmap bmp, Mat mat) {
+ bitmapToMat(bmp, mat, false);
+ }
+
+
+ /**
+ * Converts OpenCV Mat to Android Bitmap.
+ *
+ *
This function converts an image in the OpenCV Mat representation to the Android Bitmap.
+ *
The input Mat object has to be of the types 'CV_8UC1' (gray-scale), 'CV_8UC3' (RGB) or 'CV_8UC4' (RGBA).
+ *
The output Bitmap object has to be of the same size as the input Mat and of the types 'ARGB_8888' or 'RGB_565'.
+ *
This function throws an exception if the conversion fails.
+ *
+ * @param mat is a valid input Mat object of types 'CV_8UC1', 'CV_8UC3' or 'CV_8UC4'.
+ * @param bmp is a valid Bitmap object of the same size as the Mat and of type 'ARGB_8888' or 'RGB_565'.
+ * @param premultiplyAlpha is a flag, that determines, whether the Mat needs to be converted to alpha premultiplied format (like Android keeps 'ARGB_8888' bitmaps); the flag is ignored for 'RGB_565' bitmaps.
+ */
+ public static void matToBitmap(Mat mat, Bitmap bmp, boolean premultiplyAlpha) {
+ if (mat == null)
+ throw new java.lang.IllegalArgumentException("mat == null");
+ if (bmp == null)
+ throw new java.lang.IllegalArgumentException("bmp == null");
+ nMatToBitmap2(mat.nativeObj, bmp, premultiplyAlpha);
+ }
+
+ /**
+ * Short form of the matToBitmap(mat, bmp, premultiplyAlpha=false)
+ * @param mat is a valid input Mat object of the types 'CV_8UC1', 'CV_8UC3' or 'CV_8UC4'.
+ * @param bmp is a valid Bitmap object of the same size as the Mat and of type 'ARGB_8888' or 'RGB_565'.
+ */
+ public static void matToBitmap(Mat mat, Bitmap bmp) {
+ matToBitmap(mat, bmp, false);
+ }
+
+
+ private static native void nBitmapToMat2(Bitmap b, long m_addr, boolean unPremultiplyAlpha);
+
+ private static native void nMatToBitmap2(long m_addr, Bitmap b, boolean premultiplyAlpha);
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/calib3d/Calib3d.java b/openCVLibrary330/src/main/java/org/opencv/calib3d/Calib3d.java
new file mode 100644
index 00000000000..4ac7a200e6c
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/calib3d/Calib3d.java
@@ -0,0 +1,1564 @@
+
+//
+// This file is auto-generated. Please don't modify it!
+//
+package org.opencv.calib3d;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Size;
+import org.opencv.core.TermCriteria;
+import org.opencv.utils.Converters;
+
+public class Calib3d {
+
+ public static final int
+ CALIB_USE_INTRINSIC_GUESS = 1,
+ CALIB_RECOMPUTE_EXTRINSIC = 2,
+ CALIB_CHECK_COND = 4,
+ CALIB_FIX_SKEW = 8,
+ CALIB_FIX_K1 = 16,
+ CALIB_FIX_K2 = 32,
+ CALIB_FIX_K3 = 64,
+ CALIB_FIX_K4 = 128,
+ CALIB_FIX_INTRINSIC = 256,
+ CV_ITERATIVE = 0,
+ CV_EPNP = 1,
+ CV_P3P = 2,
+ CV_DLS = 3,
+ LMEDS = 4,
+ RANSAC = 8,
+ RHO = 16,
+ SOLVEPNP_ITERATIVE = 0,
+ SOLVEPNP_EPNP = 1,
+ SOLVEPNP_P3P = 2,
+ SOLVEPNP_DLS = 3,
+ SOLVEPNP_UPNP = 4,
+ SOLVEPNP_AP3P = 5,
+ SOLVEPNP_MAX_COUNT = 5+1,
+ CALIB_CB_ADAPTIVE_THRESH = 1,
+ CALIB_CB_NORMALIZE_IMAGE = 2,
+ CALIB_CB_FILTER_QUADS = 4,
+ CALIB_CB_FAST_CHECK = 8,
+ CALIB_CB_SYMMETRIC_GRID = 1,
+ CALIB_CB_ASYMMETRIC_GRID = 2,
+ CALIB_CB_CLUSTERING = 4,
+ CALIB_FIX_ASPECT_RATIO = 0x00002,
+ CALIB_FIX_PRINCIPAL_POINT = 0x00004,
+ CALIB_ZERO_TANGENT_DIST = 0x00008,
+ CALIB_FIX_FOCAL_LENGTH = 0x00010,
+ CALIB_FIX_K5 = 0x01000,
+ CALIB_FIX_K6 = 0x02000,
+ CALIB_RATIONAL_MODEL = 0x04000,
+ CALIB_THIN_PRISM_MODEL = 0x08000,
+ CALIB_FIX_S1_S2_S3_S4 = 0x10000,
+ CALIB_TILTED_MODEL = 0x40000,
+ CALIB_FIX_TAUX_TAUY = 0x80000,
+ CALIB_USE_QR = 0x100000,
+ CALIB_FIX_TANGENT_DIST = 0x200000,
+ CALIB_SAME_FOCAL_LENGTH = 0x00200,
+ CALIB_ZERO_DISPARITY = 0x00400,
+ CALIB_USE_LU = (1 << 17),
+ FM_7POINT = 1,
+ FM_8POINT = 2,
+ FM_LMEDS = 4,
+ FM_RANSAC = 8;
+
+
+ //
+ // C++: Mat estimateAffine2D(Mat from, Mat to, Mat& inliers = Mat(), int method = RANSAC, double ransacReprojThreshold = 3, size_t maxIters = 2000, double confidence = 0.99, size_t refineIters = 10)
+ //
+
+ //javadoc: estimateAffine2D(from, to, inliers, method, ransacReprojThreshold, maxIters, confidence, refineIters)
+ public static Mat estimateAffine2D(Mat from, Mat to, Mat inliers, int method, double ransacReprojThreshold, long maxIters, double confidence, long refineIters)
+ {
+
+ Mat retVal = new Mat(estimateAffine2D_0(from.nativeObj, to.nativeObj, inliers.nativeObj, method, ransacReprojThreshold, maxIters, confidence, refineIters));
+
+ return retVal;
+ }
+
+ //javadoc: estimateAffine2D(from, to)
+ public static Mat estimateAffine2D(Mat from, Mat to)
+ {
+
+ Mat retVal = new Mat(estimateAffine2D_1(from.nativeObj, to.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat estimateAffinePartial2D(Mat from, Mat to, Mat& inliers = Mat(), int method = RANSAC, double ransacReprojThreshold = 3, size_t maxIters = 2000, double confidence = 0.99, size_t refineIters = 10)
+ //
+
+ //javadoc: estimateAffinePartial2D(from, to, inliers, method, ransacReprojThreshold, maxIters, confidence, refineIters)
+ public static Mat estimateAffinePartial2D(Mat from, Mat to, Mat inliers, int method, double ransacReprojThreshold, long maxIters, double confidence, long refineIters)
+ {
+
+ Mat retVal = new Mat(estimateAffinePartial2D_0(from.nativeObj, to.nativeObj, inliers.nativeObj, method, ransacReprojThreshold, maxIters, confidence, refineIters));
+
+ return retVal;
+ }
+
+ //javadoc: estimateAffinePartial2D(from, to)
+ public static Mat estimateAffinePartial2D(Mat from, Mat to)
+ {
+
+ Mat retVal = new Mat(estimateAffinePartial2D_1(from.nativeObj, to.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat findEssentialMat(Mat points1, Mat points2, Mat cameraMatrix, int method = RANSAC, double prob = 0.999, double threshold = 1.0, Mat& mask = Mat())
+ //
+
+ //javadoc: findEssentialMat(points1, points2, cameraMatrix, method, prob, threshold, mask)
+ public static Mat findEssentialMat(Mat points1, Mat points2, Mat cameraMatrix, int method, double prob, double threshold, Mat mask)
+ {
+
+ Mat retVal = new Mat(findEssentialMat_0(points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj, method, prob, threshold, mask.nativeObj));
+
+ return retVal;
+ }
+
+ //javadoc: findEssentialMat(points1, points2, cameraMatrix, method, prob, threshold)
+ public static Mat findEssentialMat(Mat points1, Mat points2, Mat cameraMatrix, int method, double prob, double threshold)
+ {
+
+ Mat retVal = new Mat(findEssentialMat_1(points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj, method, prob, threshold));
+
+ return retVal;
+ }
+
+ //javadoc: findEssentialMat(points1, points2, cameraMatrix)
+ public static Mat findEssentialMat(Mat points1, Mat points2, Mat cameraMatrix)
+ {
+
+ Mat retVal = new Mat(findEssentialMat_2(points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat findEssentialMat(Mat points1, Mat points2, double focal = 1.0, Point2d pp = Point2d(0, 0), int method = RANSAC, double prob = 0.999, double threshold = 1.0, Mat& mask = Mat())
+ //
+
+ //javadoc: findEssentialMat(points1, points2, focal, pp, method, prob, threshold, mask)
+ public static Mat findEssentialMat(Mat points1, Mat points2, double focal, Point pp, int method, double prob, double threshold, Mat mask)
+ {
+
+ Mat retVal = new Mat(findEssentialMat_3(points1.nativeObj, points2.nativeObj, focal, pp.x, pp.y, method, prob, threshold, mask.nativeObj));
+
+ return retVal;
+ }
+
+ //javadoc: findEssentialMat(points1, points2, focal, pp, method, prob, threshold)
+ public static Mat findEssentialMat(Mat points1, Mat points2, double focal, Point pp, int method, double prob, double threshold)
+ {
+
+ Mat retVal = new Mat(findEssentialMat_4(points1.nativeObj, points2.nativeObj, focal, pp.x, pp.y, method, prob, threshold));
+
+ return retVal;
+ }
+
+ //javadoc: findEssentialMat(points1, points2)
+ public static Mat findEssentialMat(Mat points1, Mat points2)
+ {
+
+ Mat retVal = new Mat(findEssentialMat_5(points1.nativeObj, points2.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat findFundamentalMat(vector_Point2f points1, vector_Point2f points2, int method = FM_RANSAC, double param1 = 3., double param2 = 0.99, Mat& mask = Mat())
+ //
+
+ //javadoc: findFundamentalMat(points1, points2, method, param1, param2, mask)
+ public static Mat findFundamentalMat(MatOfPoint2f points1, MatOfPoint2f points2, int method, double param1, double param2, Mat mask)
+ {
+ Mat points1_mat = points1;
+ Mat points2_mat = points2;
+ Mat retVal = new Mat(findFundamentalMat_0(points1_mat.nativeObj, points2_mat.nativeObj, method, param1, param2, mask.nativeObj));
+
+ return retVal;
+ }
+
+ //javadoc: findFundamentalMat(points1, points2, method, param1, param2)
+ public static Mat findFundamentalMat(MatOfPoint2f points1, MatOfPoint2f points2, int method, double param1, double param2)
+ {
+ Mat points1_mat = points1;
+ Mat points2_mat = points2;
+ Mat retVal = new Mat(findFundamentalMat_1(points1_mat.nativeObj, points2_mat.nativeObj, method, param1, param2));
+
+ return retVal;
+ }
+
+ //javadoc: findFundamentalMat(points1, points2)
+ public static Mat findFundamentalMat(MatOfPoint2f points1, MatOfPoint2f points2)
+ {
+ Mat points1_mat = points1;
+ Mat points2_mat = points2;
+ Mat retVal = new Mat(findFundamentalMat_2(points1_mat.nativeObj, points2_mat.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat findHomography(vector_Point2f srcPoints, vector_Point2f dstPoints, int method = 0, double ransacReprojThreshold = 3, Mat& mask = Mat(), int maxIters = 2000, double confidence = 0.995)
+ //
+
+ //javadoc: findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, mask, maxIters, confidence)
+ public static Mat findHomography(MatOfPoint2f srcPoints, MatOfPoint2f dstPoints, int method, double ransacReprojThreshold, Mat mask, int maxIters, double confidence)
+ {
+ Mat srcPoints_mat = srcPoints;
+ Mat dstPoints_mat = dstPoints;
+ Mat retVal = new Mat(findHomography_0(srcPoints_mat.nativeObj, dstPoints_mat.nativeObj, method, ransacReprojThreshold, mask.nativeObj, maxIters, confidence));
+
+ return retVal;
+ }
+
+ //javadoc: findHomography(srcPoints, dstPoints, method, ransacReprojThreshold)
+ public static Mat findHomography(MatOfPoint2f srcPoints, MatOfPoint2f dstPoints, int method, double ransacReprojThreshold)
+ {
+ Mat srcPoints_mat = srcPoints;
+ Mat dstPoints_mat = dstPoints;
+ Mat retVal = new Mat(findHomography_1(srcPoints_mat.nativeObj, dstPoints_mat.nativeObj, method, ransacReprojThreshold));
+
+ return retVal;
+ }
+
+ //javadoc: findHomography(srcPoints, dstPoints)
+ public static Mat findHomography(MatOfPoint2f srcPoints, MatOfPoint2f dstPoints)
+ {
+ Mat srcPoints_mat = srcPoints;
+ Mat dstPoints_mat = dstPoints;
+ Mat retVal = new Mat(findHomography_2(srcPoints_mat.nativeObj, dstPoints_mat.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat getOptimalNewCameraMatrix(Mat cameraMatrix, Mat distCoeffs, Size imageSize, double alpha, Size newImgSize = Size(), Rect* validPixROI = 0, bool centerPrincipalPoint = false)
+ //
+
+ //javadoc: getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, alpha, newImgSize, validPixROI, centerPrincipalPoint)
+ public static Mat getOptimalNewCameraMatrix(Mat cameraMatrix, Mat distCoeffs, Size imageSize, double alpha, Size newImgSize, Rect validPixROI, boolean centerPrincipalPoint)
+ {
+ double[] validPixROI_out = new double[4];
+ Mat retVal = new Mat(getOptimalNewCameraMatrix_0(cameraMatrix.nativeObj, distCoeffs.nativeObj, imageSize.width, imageSize.height, alpha, newImgSize.width, newImgSize.height, validPixROI_out, centerPrincipalPoint));
+ if(validPixROI!=null){ validPixROI.x = (int)validPixROI_out[0]; validPixROI.y = (int)validPixROI_out[1]; validPixROI.width = (int)validPixROI_out[2]; validPixROI.height = (int)validPixROI_out[3]; }
+ return retVal;
+ }
+
+ //javadoc: getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, alpha)
+ public static Mat getOptimalNewCameraMatrix(Mat cameraMatrix, Mat distCoeffs, Size imageSize, double alpha)
+ {
+
+ Mat retVal = new Mat(getOptimalNewCameraMatrix_1(cameraMatrix.nativeObj, distCoeffs.nativeObj, imageSize.width, imageSize.height, alpha));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Mat initCameraMatrix2D(vector_vector_Point3f objectPoints, vector_vector_Point2f imagePoints, Size imageSize, double aspectRatio = 1.0)
+ //
+
+ //javadoc: initCameraMatrix2D(objectPoints, imagePoints, imageSize, aspectRatio)
+ public static Mat initCameraMatrix2D(List objectPoints, List imagePoints, Size imageSize, double aspectRatio)
+ {
+ List objectPoints_tmplm = new ArrayList((objectPoints != null) ? objectPoints.size() : 0);
+ Mat objectPoints_mat = Converters.vector_vector_Point3f_to_Mat(objectPoints, objectPoints_tmplm);
+ List imagePoints_tmplm = new ArrayList((imagePoints != null) ? imagePoints.size() : 0);
+ Mat imagePoints_mat = Converters.vector_vector_Point2f_to_Mat(imagePoints, imagePoints_tmplm);
+ Mat retVal = new Mat(initCameraMatrix2D_0(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, aspectRatio));
+
+ return retVal;
+ }
+
+ //javadoc: initCameraMatrix2D(objectPoints, imagePoints, imageSize)
+ public static Mat initCameraMatrix2D(List objectPoints, List imagePoints, Size imageSize)
+ {
+ List objectPoints_tmplm = new ArrayList((objectPoints != null) ? objectPoints.size() : 0);
+ Mat objectPoints_mat = Converters.vector_vector_Point3f_to_Mat(objectPoints, objectPoints_tmplm);
+ List imagePoints_tmplm = new ArrayList((imagePoints != null) ? imagePoints.size() : 0);
+ Mat imagePoints_mat = Converters.vector_vector_Point2f_to_Mat(imagePoints, imagePoints_tmplm);
+ Mat retVal = new Mat(initCameraMatrix2D_1(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Rect getValidDisparityROI(Rect roi1, Rect roi2, int minDisparity, int numberOfDisparities, int SADWindowSize)
+ //
+
+ //javadoc: getValidDisparityROI(roi1, roi2, minDisparity, numberOfDisparities, SADWindowSize)
+ public static Rect getValidDisparityROI(Rect roi1, Rect roi2, int minDisparity, int numberOfDisparities, int SADWindowSize)
+ {
+
+ Rect retVal = new Rect(getValidDisparityROI_0(roi1.x, roi1.y, roi1.width, roi1.height, roi2.x, roi2.y, roi2.width, roi2.height, minDisparity, numberOfDisparities, SADWindowSize));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Vec3d RQDecomp3x3(Mat src, Mat& mtxR, Mat& mtxQ, Mat& Qx = Mat(), Mat& Qy = Mat(), Mat& Qz = Mat())
+ //
+
+ //javadoc: RQDecomp3x3(src, mtxR, mtxQ, Qx, Qy, Qz)
+ public static double[] RQDecomp3x3(Mat src, Mat mtxR, Mat mtxQ, Mat Qx, Mat Qy, Mat Qz)
+ {
+
+ double[] retVal = RQDecomp3x3_0(src.nativeObj, mtxR.nativeObj, mtxQ.nativeObj, Qx.nativeObj, Qy.nativeObj, Qz.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: RQDecomp3x3(src, mtxR, mtxQ)
+ public static double[] RQDecomp3x3(Mat src, Mat mtxR, Mat mtxQ)
+ {
+
+ double[] retVal = RQDecomp3x3_1(src.nativeObj, mtxR.nativeObj, mtxQ.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool findChessboardCorners(Mat image, Size patternSize, vector_Point2f& corners, int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE)
+ //
+
+ //javadoc: findChessboardCorners(image, patternSize, corners, flags)
+ public static boolean findChessboardCorners(Mat image, Size patternSize, MatOfPoint2f corners, int flags)
+ {
+ Mat corners_mat = corners;
+ boolean retVal = findChessboardCorners_0(image.nativeObj, patternSize.width, patternSize.height, corners_mat.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: findChessboardCorners(image, patternSize, corners)
+ public static boolean findChessboardCorners(Mat image, Size patternSize, MatOfPoint2f corners)
+ {
+ Mat corners_mat = corners;
+ boolean retVal = findChessboardCorners_1(image.nativeObj, patternSize.width, patternSize.height, corners_mat.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool findCirclesGrid(Mat image, Size patternSize, Mat& centers, int flags, Ptr_FeatureDetector blobDetector, CirclesGridFinderParameters parameters)
+ //
+
+ // Unknown type 'Ptr_FeatureDetector' (I), skipping the function
+
+
+ //
+ // C++: bool findCirclesGrid(Mat image, Size patternSize, Mat& centers, int flags = CALIB_CB_SYMMETRIC_GRID, Ptr_FeatureDetector blobDetector = SimpleBlobDetector::create())
+ //
+
+ //javadoc: findCirclesGrid(image, patternSize, centers, flags)
+ public static boolean findCirclesGrid(Mat image, Size patternSize, Mat centers, int flags)
+ {
+
+ boolean retVal = findCirclesGrid_0(image.nativeObj, patternSize.width, patternSize.height, centers.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: findCirclesGrid(image, patternSize, centers)
+ public static boolean findCirclesGrid(Mat image, Size patternSize, Mat centers)
+ {
+
+ boolean retVal = findCirclesGrid_1(image.nativeObj, patternSize.width, patternSize.height, centers.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool solvePnP(vector_Point3f objectPoints, vector_Point2f imagePoints, Mat cameraMatrix, vector_double distCoeffs, Mat& rvec, Mat& tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE)
+ //
+
+ //javadoc: solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags)
+ public static boolean solvePnP(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat cameraMatrix, MatOfDouble distCoeffs, Mat rvec, Mat tvec, boolean useExtrinsicGuess, int flags)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat imagePoints_mat = imagePoints;
+ Mat distCoeffs_mat = distCoeffs;
+ boolean retVal = solvePnP_0(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, cameraMatrix.nativeObj, distCoeffs_mat.nativeObj, rvec.nativeObj, tvec.nativeObj, useExtrinsicGuess, flags);
+
+ return retVal;
+ }
+
+ //javadoc: solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec)
+ public static boolean solvePnP(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat cameraMatrix, MatOfDouble distCoeffs, Mat rvec, Mat tvec)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat imagePoints_mat = imagePoints;
+ Mat distCoeffs_mat = distCoeffs;
+ boolean retVal = solvePnP_1(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, cameraMatrix.nativeObj, distCoeffs_mat.nativeObj, rvec.nativeObj, tvec.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool solvePnPRansac(vector_Point3f objectPoints, vector_Point2f imagePoints, Mat cameraMatrix, vector_double distCoeffs, Mat& rvec, Mat& tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, double confidence = 0.99, Mat& inliers = Mat(), int flags = SOLVEPNP_ITERATIVE)
+ //
+
+ //javadoc: solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, iterationsCount, reprojectionError, confidence, inliers, flags)
+ public static boolean solvePnPRansac(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat cameraMatrix, MatOfDouble distCoeffs, Mat rvec, Mat tvec, boolean useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, Mat inliers, int flags)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat imagePoints_mat = imagePoints;
+ Mat distCoeffs_mat = distCoeffs;
+ boolean retVal = solvePnPRansac_0(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, cameraMatrix.nativeObj, distCoeffs_mat.nativeObj, rvec.nativeObj, tvec.nativeObj, useExtrinsicGuess, iterationsCount, reprojectionError, confidence, inliers.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec)
+ public static boolean solvePnPRansac(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat cameraMatrix, MatOfDouble distCoeffs, Mat rvec, Mat tvec)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat imagePoints_mat = imagePoints;
+ Mat distCoeffs_mat = distCoeffs;
+ boolean retVal = solvePnPRansac_1(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, cameraMatrix.nativeObj, distCoeffs_mat.nativeObj, rvec.nativeObj, tvec.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool stereoRectifyUncalibrated(Mat points1, Mat points2, Mat F, Size imgSize, Mat& H1, Mat& H2, double threshold = 5)
+ //
+
+ //javadoc: stereoRectifyUncalibrated(points1, points2, F, imgSize, H1, H2, threshold)
+ public static boolean stereoRectifyUncalibrated(Mat points1, Mat points2, Mat F, Size imgSize, Mat H1, Mat H2, double threshold)
+ {
+
+ boolean retVal = stereoRectifyUncalibrated_0(points1.nativeObj, points2.nativeObj, F.nativeObj, imgSize.width, imgSize.height, H1.nativeObj, H2.nativeObj, threshold);
+
+ return retVal;
+ }
+
+ //javadoc: stereoRectifyUncalibrated(points1, points2, F, imgSize, H1, H2)
+ public static boolean stereoRectifyUncalibrated(Mat points1, Mat points2, Mat F, Size imgSize, Mat H1, Mat H2)
+ {
+
+ boolean retVal = stereoRectifyUncalibrated_1(points1.nativeObj, points2.nativeObj, F.nativeObj, imgSize.width, imgSize.height, H1.nativeObj, H2.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double calibrateCamera(vector_Mat objectPoints, vector_Mat imagePoints, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, vector_Mat& rvecs, vector_Mat& tvecs, Mat& stdDeviationsIntrinsics, Mat& stdDeviationsExtrinsics, Mat& perViewErrors, int flags = 0, TermCriteria criteria = TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON))
+ //
+
+ //javadoc: calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors, flags, criteria)
+ public static double calibrateCameraExtended(List objectPoints, List imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs, Mat stdDeviationsIntrinsics, Mat stdDeviationsExtrinsics, Mat perViewErrors, int flags, TermCriteria criteria)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrateCameraExtended_0(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, stdDeviationsIntrinsics.nativeObj, stdDeviationsExtrinsics.nativeObj, perViewErrors.nativeObj, flags, criteria.type, criteria.maxCount, criteria.epsilon);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+ //javadoc: calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors, flags)
+ public static double calibrateCameraExtended(List objectPoints, List imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs, Mat stdDeviationsIntrinsics, Mat stdDeviationsExtrinsics, Mat perViewErrors, int flags)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrateCameraExtended_1(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, stdDeviationsIntrinsics.nativeObj, stdDeviationsExtrinsics.nativeObj, perViewErrors.nativeObj, flags);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+ //javadoc: calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors)
+ public static double calibrateCameraExtended(List objectPoints, List imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs, Mat stdDeviationsIntrinsics, Mat stdDeviationsExtrinsics, Mat perViewErrors)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrateCameraExtended_2(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, stdDeviationsIntrinsics.nativeObj, stdDeviationsExtrinsics.nativeObj, perViewErrors.nativeObj);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+
+ //
+ // C++: double calibrateCamera(vector_Mat objectPoints, vector_Mat imagePoints, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, vector_Mat& rvecs, vector_Mat& tvecs, int flags = 0, TermCriteria criteria = TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON))
+ //
+
+ //javadoc: calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags, criteria)
+ public static double calibrateCamera(List objectPoints, List imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs, int flags, TermCriteria criteria)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrateCamera_0(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, flags, criteria.type, criteria.maxCount, criteria.epsilon);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+ //javadoc: calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags)
+ public static double calibrateCamera(List objectPoints, List imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs, int flags)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrateCamera_1(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, flags);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+ //javadoc: calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs)
+ public static double calibrateCamera(List objectPoints, List imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrateCamera_2(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, imageSize.width, imageSize.height, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+
+ //
+ // C++: double sampsonDistance(Mat pt1, Mat pt2, Mat F)
+ //
+
+ //javadoc: sampsonDistance(pt1, pt2, F)
+ public static double sampsonDistance(Mat pt1, Mat pt2, Mat F)
+ {
+
+ double retVal = sampsonDistance_0(pt1.nativeObj, pt2.nativeObj, F.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double stereoCalibrate(vector_Mat objectPoints, vector_Mat imagePoints1, vector_Mat imagePoints2, Mat& cameraMatrix1, Mat& distCoeffs1, Mat& cameraMatrix2, Mat& distCoeffs2, Size imageSize, Mat& R, Mat& T, Mat& E, Mat& F, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6))
+ //
+
+ //javadoc: stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, E, F, flags, criteria)
+ public static double stereoCalibrate(List objectPoints, List imagePoints1, List imagePoints2, Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat E, Mat F, int flags, TermCriteria criteria)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints1_mat = Converters.vector_Mat_to_Mat(imagePoints1);
+ Mat imagePoints2_mat = Converters.vector_Mat_to_Mat(imagePoints2);
+ double retVal = stereoCalibrate_0(objectPoints_mat.nativeObj, imagePoints1_mat.nativeObj, imagePoints2_mat.nativeObj, cameraMatrix1.nativeObj, distCoeffs1.nativeObj, cameraMatrix2.nativeObj, distCoeffs2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, E.nativeObj, F.nativeObj, flags, criteria.type, criteria.maxCount, criteria.epsilon);
+
+ return retVal;
+ }
+
+ //javadoc: stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, E, F, flags)
+ public static double stereoCalibrate(List objectPoints, List imagePoints1, List imagePoints2, Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat E, Mat F, int flags)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints1_mat = Converters.vector_Mat_to_Mat(imagePoints1);
+ Mat imagePoints2_mat = Converters.vector_Mat_to_Mat(imagePoints2);
+ double retVal = stereoCalibrate_1(objectPoints_mat.nativeObj, imagePoints1_mat.nativeObj, imagePoints2_mat.nativeObj, cameraMatrix1.nativeObj, distCoeffs1.nativeObj, cameraMatrix2.nativeObj, distCoeffs2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, E.nativeObj, F.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, E, F)
+ public static double stereoCalibrate(List objectPoints, List imagePoints1, List imagePoints2, Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat E, Mat F)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints1_mat = Converters.vector_Mat_to_Mat(imagePoints1);
+ Mat imagePoints2_mat = Converters.vector_Mat_to_Mat(imagePoints2);
+ double retVal = stereoCalibrate_2(objectPoints_mat.nativeObj, imagePoints1_mat.nativeObj, imagePoints2_mat.nativeObj, cameraMatrix1.nativeObj, distCoeffs1.nativeObj, cameraMatrix2.nativeObj, distCoeffs2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, E.nativeObj, F.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double calibrate(vector_Mat objectPoints, vector_Mat imagePoints, Size image_size, Mat& K, Mat& D, vector_Mat& rvecs, vector_Mat& tvecs, int flags = 0, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON))
+ //
+
+ //javadoc: calibrate(objectPoints, imagePoints, image_size, K, D, rvecs, tvecs, flags, criteria)
+ public static double calibrate(List objectPoints, List imagePoints, Size image_size, Mat K, Mat D, List rvecs, List tvecs, int flags, TermCriteria criteria)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrate_0(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, image_size.width, image_size.height, K.nativeObj, D.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, flags, criteria.type, criteria.maxCount, criteria.epsilon);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+ //javadoc: calibrate(objectPoints, imagePoints, image_size, K, D, rvecs, tvecs, flags)
+ public static double calibrate(List objectPoints, List imagePoints, Size image_size, Mat K, Mat D, List rvecs, List tvecs, int flags)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrate_1(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, image_size.width, image_size.height, K.nativeObj, D.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, flags);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+ //javadoc: calibrate(objectPoints, imagePoints, image_size, K, D, rvecs, tvecs)
+ public static double calibrate(List objectPoints, List imagePoints, Size image_size, Mat K, Mat D, List rvecs, List tvecs)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints_mat = Converters.vector_Mat_to_Mat(imagePoints);
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ double retVal = calibrate_2(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, image_size.width, image_size.height, K.nativeObj, D.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+
+ //
+ // C++: double stereoCalibrate(vector_Mat objectPoints, vector_Mat imagePoints1, vector_Mat imagePoints2, Mat& K1, Mat& D1, Mat& K2, Mat& D2, Size imageSize, Mat& R, Mat& T, int flags = fisheye::CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON))
+ //
+
+ //javadoc: stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T, flags, criteria)
+ public static double stereoCalibrate(List objectPoints, List imagePoints1, List imagePoints2, Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat T, int flags, TermCriteria criteria)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints1_mat = Converters.vector_Mat_to_Mat(imagePoints1);
+ Mat imagePoints2_mat = Converters.vector_Mat_to_Mat(imagePoints2);
+ double retVal = stereoCalibrate_3(objectPoints_mat.nativeObj, imagePoints1_mat.nativeObj, imagePoints2_mat.nativeObj, K1.nativeObj, D1.nativeObj, K2.nativeObj, D2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, flags, criteria.type, criteria.maxCount, criteria.epsilon);
+
+ return retVal;
+ }
+
+ //javadoc: stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T, flags)
+ public static double stereoCalibrate(List objectPoints, List imagePoints1, List imagePoints2, Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat T, int flags)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints1_mat = Converters.vector_Mat_to_Mat(imagePoints1);
+ Mat imagePoints2_mat = Converters.vector_Mat_to_Mat(imagePoints2);
+ double retVal = stereoCalibrate_4(objectPoints_mat.nativeObj, imagePoints1_mat.nativeObj, imagePoints2_mat.nativeObj, K1.nativeObj, D1.nativeObj, K2.nativeObj, D2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T)
+ public static double stereoCalibrate(List objectPoints, List imagePoints1, List imagePoints2, Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat T)
+ {
+ Mat objectPoints_mat = Converters.vector_Mat_to_Mat(objectPoints);
+ Mat imagePoints1_mat = Converters.vector_Mat_to_Mat(imagePoints1);
+ Mat imagePoints2_mat = Converters.vector_Mat_to_Mat(imagePoints2);
+ double retVal = stereoCalibrate_5(objectPoints_mat.nativeObj, imagePoints1_mat.nativeObj, imagePoints2_mat.nativeObj, K1.nativeObj, D1.nativeObj, K2.nativeObj, D2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: float rectify3Collinear(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Mat cameraMatrix3, Mat distCoeffs3, vector_Mat imgpt1, vector_Mat imgpt3, Size imageSize, Mat R12, Mat T12, Mat R13, Mat T13, Mat& R1, Mat& R2, Mat& R3, Mat& P1, Mat& P2, Mat& P3, Mat& Q, double alpha, Size newImgSize, Rect* roi1, Rect* roi2, int flags)
+ //
+
+ //javadoc: rectify3Collinear(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, cameraMatrix3, distCoeffs3, imgpt1, imgpt3, imageSize, R12, T12, R13, T13, R1, R2, R3, P1, P2, P3, Q, alpha, newImgSize, roi1, roi2, flags)
+ public static float rectify3Collinear(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Mat cameraMatrix3, Mat distCoeffs3, List imgpt1, List imgpt3, Size imageSize, Mat R12, Mat T12, Mat R13, Mat T13, Mat R1, Mat R2, Mat R3, Mat P1, Mat P2, Mat P3, Mat Q, double alpha, Size newImgSize, Rect roi1, Rect roi2, int flags)
+ {
+ Mat imgpt1_mat = Converters.vector_Mat_to_Mat(imgpt1);
+ Mat imgpt3_mat = Converters.vector_Mat_to_Mat(imgpt3);
+ double[] roi1_out = new double[4];
+ double[] roi2_out = new double[4];
+ float retVal = rectify3Collinear_0(cameraMatrix1.nativeObj, distCoeffs1.nativeObj, cameraMatrix2.nativeObj, distCoeffs2.nativeObj, cameraMatrix3.nativeObj, distCoeffs3.nativeObj, imgpt1_mat.nativeObj, imgpt3_mat.nativeObj, imageSize.width, imageSize.height, R12.nativeObj, T12.nativeObj, R13.nativeObj, T13.nativeObj, R1.nativeObj, R2.nativeObj, R3.nativeObj, P1.nativeObj, P2.nativeObj, P3.nativeObj, Q.nativeObj, alpha, newImgSize.width, newImgSize.height, roi1_out, roi2_out, flags);
+ if(roi1!=null){ roi1.x = (int)roi1_out[0]; roi1.y = (int)roi1_out[1]; roi1.width = (int)roi1_out[2]; roi1.height = (int)roi1_out[3]; }
+ if(roi2!=null){ roi2.x = (int)roi2_out[0]; roi2.y = (int)roi2_out[1]; roi2.width = (int)roi2_out[2]; roi2.height = (int)roi2_out[3]; }
+ return retVal;
+ }
+
+
+ //
+ // C++: int decomposeHomographyMat(Mat H, Mat K, vector_Mat& rotations, vector_Mat& translations, vector_Mat& normals)
+ //
+
+ //javadoc: decomposeHomographyMat(H, K, rotations, translations, normals)
+ public static int decomposeHomographyMat(Mat H, Mat K, List rotations, List translations, List normals)
+ {
+ Mat rotations_mat = new Mat();
+ Mat translations_mat = new Mat();
+ Mat normals_mat = new Mat();
+ int retVal = decomposeHomographyMat_0(H.nativeObj, K.nativeObj, rotations_mat.nativeObj, translations_mat.nativeObj, normals_mat.nativeObj);
+ Converters.Mat_to_vector_Mat(rotations_mat, rotations);
+ rotations_mat.release();
+ Converters.Mat_to_vector_Mat(translations_mat, translations);
+ translations_mat.release();
+ Converters.Mat_to_vector_Mat(normals_mat, normals);
+ normals_mat.release();
+ return retVal;
+ }
+
+
+ //
+ // C++: int estimateAffine3D(Mat src, Mat dst, Mat& out, Mat& inliers, double ransacThreshold = 3, double confidence = 0.99)
+ //
+
+ //javadoc: estimateAffine3D(src, dst, out, inliers, ransacThreshold, confidence)
+ public static int estimateAffine3D(Mat src, Mat dst, Mat out, Mat inliers, double ransacThreshold, double confidence)
+ {
+
+ int retVal = estimateAffine3D_0(src.nativeObj, dst.nativeObj, out.nativeObj, inliers.nativeObj, ransacThreshold, confidence);
+
+ return retVal;
+ }
+
+ //javadoc: estimateAffine3D(src, dst, out, inliers)
+ public static int estimateAffine3D(Mat src, Mat dst, Mat out, Mat inliers)
+ {
+
+ int retVal = estimateAffine3D_1(src.nativeObj, dst.nativeObj, out.nativeObj, inliers.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int recoverPose(Mat E, Mat points1, Mat points2, Mat& R, Mat& t, double focal = 1.0, Point2d pp = Point2d(0, 0), Mat& mask = Mat())
+ //
+
+ //javadoc: recoverPose(E, points1, points2, R, t, focal, pp, mask)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat R, Mat t, double focal, Point pp, Mat mask)
+ {
+
+ int retVal = recoverPose_0(E.nativeObj, points1.nativeObj, points2.nativeObj, R.nativeObj, t.nativeObj, focal, pp.x, pp.y, mask.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: recoverPose(E, points1, points2, R, t, focal, pp)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat R, Mat t, double focal, Point pp)
+ {
+
+ int retVal = recoverPose_1(E.nativeObj, points1.nativeObj, points2.nativeObj, R.nativeObj, t.nativeObj, focal, pp.x, pp.y);
+
+ return retVal;
+ }
+
+ //javadoc: recoverPose(E, points1, points2, R, t)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat R, Mat t)
+ {
+
+ int retVal = recoverPose_2(E.nativeObj, points1.nativeObj, points2.nativeObj, R.nativeObj, t.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat& R, Mat& t, Mat& mask = Mat())
+ //
+
+ //javadoc: recoverPose(E, points1, points2, cameraMatrix, R, t, mask)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat R, Mat t, Mat mask)
+ {
+
+ int retVal = recoverPose_3(E.nativeObj, points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj, R.nativeObj, t.nativeObj, mask.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: recoverPose(E, points1, points2, cameraMatrix, R, t)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat R, Mat t)
+ {
+
+ int retVal = recoverPose_4(E.nativeObj, points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj, R.nativeObj, t.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat& R, Mat& t, double distanceThresh, Mat& mask = Mat(), Mat& triangulatedPoints = Mat())
+ //
+
+ //javadoc: recoverPose(E, points1, points2, cameraMatrix, R, t, distanceThresh, mask, triangulatedPoints)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat R, Mat t, double distanceThresh, Mat mask, Mat triangulatedPoints)
+ {
+
+ int retVal = recoverPose_5(E.nativeObj, points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj, R.nativeObj, t.nativeObj, distanceThresh, mask.nativeObj, triangulatedPoints.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: recoverPose(E, points1, points2, cameraMatrix, R, t, distanceThresh)
+ public static int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat R, Mat t, double distanceThresh)
+ {
+
+ int retVal = recoverPose_6(E.nativeObj, points1.nativeObj, points2.nativeObj, cameraMatrix.nativeObj, R.nativeObj, t.nativeObj, distanceThresh);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int solveP3P(Mat objectPoints, Mat imagePoints, Mat cameraMatrix, Mat distCoeffs, vector_Mat& rvecs, vector_Mat& tvecs, int flags)
+ //
+
+ //javadoc: solveP3P(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs, tvecs, flags)
+ public static int solveP3P(Mat objectPoints, Mat imagePoints, Mat cameraMatrix, Mat distCoeffs, List rvecs, List tvecs, int flags)
+ {
+ Mat rvecs_mat = new Mat();
+ Mat tvecs_mat = new Mat();
+ int retVal = solveP3P_0(objectPoints.nativeObj, imagePoints.nativeObj, cameraMatrix.nativeObj, distCoeffs.nativeObj, rvecs_mat.nativeObj, tvecs_mat.nativeObj, flags);
+ Converters.Mat_to_vector_Mat(rvecs_mat, rvecs);
+ rvecs_mat.release();
+ Converters.Mat_to_vector_Mat(tvecs_mat, tvecs);
+ tvecs_mat.release();
+ return retVal;
+ }
+
+
+ //
+ // C++: void Rodrigues(Mat src, Mat& dst, Mat& jacobian = Mat())
+ //
+
+ //javadoc: Rodrigues(src, dst, jacobian)
+ public static void Rodrigues(Mat src, Mat dst, Mat jacobian)
+ {
+
+ Rodrigues_0(src.nativeObj, dst.nativeObj, jacobian.nativeObj);
+
+ return;
+ }
+
+ //javadoc: Rodrigues(src, dst)
+ public static void Rodrigues(Mat src, Mat dst)
+ {
+
+ Rodrigues_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void calibrationMatrixValues(Mat cameraMatrix, Size imageSize, double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, Point2d& principalPoint, double& aspectRatio)
+ //
+
+ //javadoc: calibrationMatrixValues(cameraMatrix, imageSize, apertureWidth, apertureHeight, fovx, fovy, focalLength, principalPoint, aspectRatio)
+ public static void calibrationMatrixValues(Mat cameraMatrix, Size imageSize, double apertureWidth, double apertureHeight, double[] fovx, double[] fovy, double[] focalLength, Point principalPoint, double[] aspectRatio)
+ {
+ double[] fovx_out = new double[1];
+ double[] fovy_out = new double[1];
+ double[] focalLength_out = new double[1];
+ double[] principalPoint_out = new double[2];
+ double[] aspectRatio_out = new double[1];
+ calibrationMatrixValues_0(cameraMatrix.nativeObj, imageSize.width, imageSize.height, apertureWidth, apertureHeight, fovx_out, fovy_out, focalLength_out, principalPoint_out, aspectRatio_out);
+ if(fovx!=null) fovx[0] = (double)fovx_out[0];
+ if(fovy!=null) fovy[0] = (double)fovy_out[0];
+ if(focalLength!=null) focalLength[0] = (double)focalLength_out[0];
+ if(principalPoint!=null){ principalPoint.x = principalPoint_out[0]; principalPoint.y = principalPoint_out[1]; }
+ if(aspectRatio!=null) aspectRatio[0] = (double)aspectRatio_out[0];
+ return;
+ }
+
+
+ //
+ // C++: void composeRT(Mat rvec1, Mat tvec1, Mat rvec2, Mat tvec2, Mat& rvec3, Mat& tvec3, Mat& dr3dr1 = Mat(), Mat& dr3dt1 = Mat(), Mat& dr3dr2 = Mat(), Mat& dr3dt2 = Mat(), Mat& dt3dr1 = Mat(), Mat& dt3dt1 = Mat(), Mat& dt3dr2 = Mat(), Mat& dt3dt2 = Mat())
+ //
+
+ //javadoc: composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3, dr3dr1, dr3dt1, dr3dr2, dr3dt2, dt3dr1, dt3dt1, dt3dr2, dt3dt2)
+ public static void composeRT(Mat rvec1, Mat tvec1, Mat rvec2, Mat tvec2, Mat rvec3, Mat tvec3, Mat dr3dr1, Mat dr3dt1, Mat dr3dr2, Mat dr3dt2, Mat dt3dr1, Mat dt3dt1, Mat dt3dr2, Mat dt3dt2)
+ {
+
+ composeRT_0(rvec1.nativeObj, tvec1.nativeObj, rvec2.nativeObj, tvec2.nativeObj, rvec3.nativeObj, tvec3.nativeObj, dr3dr1.nativeObj, dr3dt1.nativeObj, dr3dr2.nativeObj, dr3dt2.nativeObj, dt3dr1.nativeObj, dt3dt1.nativeObj, dt3dr2.nativeObj, dt3dt2.nativeObj);
+
+ return;
+ }
+
+ //javadoc: composeRT(rvec1, tvec1, rvec2, tvec2, rvec3, tvec3)
+ public static void composeRT(Mat rvec1, Mat tvec1, Mat rvec2, Mat tvec2, Mat rvec3, Mat tvec3)
+ {
+
+ composeRT_1(rvec1.nativeObj, tvec1.nativeObj, rvec2.nativeObj, tvec2.nativeObj, rvec3.nativeObj, tvec3.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void computeCorrespondEpilines(Mat points, int whichImage, Mat F, Mat& lines)
+ //
+
+ //javadoc: computeCorrespondEpilines(points, whichImage, F, lines)
+ public static void computeCorrespondEpilines(Mat points, int whichImage, Mat F, Mat lines)
+ {
+
+ computeCorrespondEpilines_0(points.nativeObj, whichImage, F.nativeObj, lines.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void convertPointsFromHomogeneous(Mat src, Mat& dst)
+ //
+
+ //javadoc: convertPointsFromHomogeneous(src, dst)
+ public static void convertPointsFromHomogeneous(Mat src, Mat dst)
+ {
+
+ convertPointsFromHomogeneous_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void convertPointsToHomogeneous(Mat src, Mat& dst)
+ //
+
+ //javadoc: convertPointsToHomogeneous(src, dst)
+ public static void convertPointsToHomogeneous(Mat src, Mat dst)
+ {
+
+ convertPointsToHomogeneous_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void correctMatches(Mat F, Mat points1, Mat points2, Mat& newPoints1, Mat& newPoints2)
+ //
+
+ //javadoc: correctMatches(F, points1, points2, newPoints1, newPoints2)
+ public static void correctMatches(Mat F, Mat points1, Mat points2, Mat newPoints1, Mat newPoints2)
+ {
+
+ correctMatches_0(F.nativeObj, points1.nativeObj, points2.nativeObj, newPoints1.nativeObj, newPoints2.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void decomposeEssentialMat(Mat E, Mat& R1, Mat& R2, Mat& t)
+ //
+
+ //javadoc: decomposeEssentialMat(E, R1, R2, t)
+ public static void decomposeEssentialMat(Mat E, Mat R1, Mat R2, Mat t)
+ {
+
+ decomposeEssentialMat_0(E.nativeObj, R1.nativeObj, R2.nativeObj, t.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void decomposeProjectionMatrix(Mat projMatrix, Mat& cameraMatrix, Mat& rotMatrix, Mat& transVect, Mat& rotMatrixX = Mat(), Mat& rotMatrixY = Mat(), Mat& rotMatrixZ = Mat(), Mat& eulerAngles = Mat())
+ //
+
+ //javadoc: decomposeProjectionMatrix(projMatrix, cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ, eulerAngles)
+ public static void decomposeProjectionMatrix(Mat projMatrix, Mat cameraMatrix, Mat rotMatrix, Mat transVect, Mat rotMatrixX, Mat rotMatrixY, Mat rotMatrixZ, Mat eulerAngles)
+ {
+
+ decomposeProjectionMatrix_0(projMatrix.nativeObj, cameraMatrix.nativeObj, rotMatrix.nativeObj, transVect.nativeObj, rotMatrixX.nativeObj, rotMatrixY.nativeObj, rotMatrixZ.nativeObj, eulerAngles.nativeObj);
+
+ return;
+ }
+
+ //javadoc: decomposeProjectionMatrix(projMatrix, cameraMatrix, rotMatrix, transVect)
+ public static void decomposeProjectionMatrix(Mat projMatrix, Mat cameraMatrix, Mat rotMatrix, Mat transVect)
+ {
+
+ decomposeProjectionMatrix_1(projMatrix.nativeObj, cameraMatrix.nativeObj, rotMatrix.nativeObj, transVect.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void drawChessboardCorners(Mat& image, Size patternSize, vector_Point2f corners, bool patternWasFound)
+ //
+
+ //javadoc: drawChessboardCorners(image, patternSize, corners, patternWasFound)
+ public static void drawChessboardCorners(Mat image, Size patternSize, MatOfPoint2f corners, boolean patternWasFound)
+ {
+ Mat corners_mat = corners;
+ drawChessboardCorners_0(image.nativeObj, patternSize.width, patternSize.height, corners_mat.nativeObj, patternWasFound);
+
+ return;
+ }
+
+
+ //
+ // C++: void filterSpeckles(Mat& img, double newVal, int maxSpeckleSize, double maxDiff, Mat& buf = Mat())
+ //
+
+ //javadoc: filterSpeckles(img, newVal, maxSpeckleSize, maxDiff, buf)
+ public static void filterSpeckles(Mat img, double newVal, int maxSpeckleSize, double maxDiff, Mat buf)
+ {
+
+ filterSpeckles_0(img.nativeObj, newVal, maxSpeckleSize, maxDiff, buf.nativeObj);
+
+ return;
+ }
+
+ //javadoc: filterSpeckles(img, newVal, maxSpeckleSize, maxDiff)
+ public static void filterSpeckles(Mat img, double newVal, int maxSpeckleSize, double maxDiff)
+ {
+
+ filterSpeckles_1(img.nativeObj, newVal, maxSpeckleSize, maxDiff);
+
+ return;
+ }
+
+
+ //
+ // C++: void matMulDeriv(Mat A, Mat B, Mat& dABdA, Mat& dABdB)
+ //
+
+ //javadoc: matMulDeriv(A, B, dABdA, dABdB)
+ public static void matMulDeriv(Mat A, Mat B, Mat dABdA, Mat dABdB)
+ {
+
+ matMulDeriv_0(A.nativeObj, B.nativeObj, dABdA.nativeObj, dABdB.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void projectPoints(vector_Point3f objectPoints, Mat rvec, Mat tvec, Mat cameraMatrix, vector_double distCoeffs, vector_Point2f& imagePoints, Mat& jacobian = Mat(), double aspectRatio = 0)
+ //
+
+ //javadoc: projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, jacobian, aspectRatio)
+ public static void projectPoints(MatOfPoint3f objectPoints, Mat rvec, Mat tvec, Mat cameraMatrix, MatOfDouble distCoeffs, MatOfPoint2f imagePoints, Mat jacobian, double aspectRatio)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat distCoeffs_mat = distCoeffs;
+ Mat imagePoints_mat = imagePoints;
+ projectPoints_0(objectPoints_mat.nativeObj, rvec.nativeObj, tvec.nativeObj, cameraMatrix.nativeObj, distCoeffs_mat.nativeObj, imagePoints_mat.nativeObj, jacobian.nativeObj, aspectRatio);
+
+ return;
+ }
+
+ //javadoc: projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints)
+ public static void projectPoints(MatOfPoint3f objectPoints, Mat rvec, Mat tvec, Mat cameraMatrix, MatOfDouble distCoeffs, MatOfPoint2f imagePoints)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat distCoeffs_mat = distCoeffs;
+ Mat imagePoints_mat = imagePoints;
+ projectPoints_1(objectPoints_mat.nativeObj, rvec.nativeObj, tvec.nativeObj, cameraMatrix.nativeObj, distCoeffs_mat.nativeObj, imagePoints_mat.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void reprojectImageTo3D(Mat disparity, Mat& _3dImage, Mat Q, bool handleMissingValues = false, int ddepth = -1)
+ //
+
+ //javadoc: reprojectImageTo3D(disparity, _3dImage, Q, handleMissingValues, ddepth)
+ public static void reprojectImageTo3D(Mat disparity, Mat _3dImage, Mat Q, boolean handleMissingValues, int ddepth)
+ {
+
+ reprojectImageTo3D_0(disparity.nativeObj, _3dImage.nativeObj, Q.nativeObj, handleMissingValues, ddepth);
+
+ return;
+ }
+
+ //javadoc: reprojectImageTo3D(disparity, _3dImage, Q, handleMissingValues)
+ public static void reprojectImageTo3D(Mat disparity, Mat _3dImage, Mat Q, boolean handleMissingValues)
+ {
+
+ reprojectImageTo3D_1(disparity.nativeObj, _3dImage.nativeObj, Q.nativeObj, handleMissingValues);
+
+ return;
+ }
+
+ //javadoc: reprojectImageTo3D(disparity, _3dImage, Q)
+ public static void reprojectImageTo3D(Mat disparity, Mat _3dImage, Mat Q)
+ {
+
+ reprojectImageTo3D_2(disparity.nativeObj, _3dImage.nativeObj, Q.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void stereoRectify(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q, int flags = CALIB_ZERO_DISPARITY, double alpha = -1, Size newImageSize = Size(), Rect* validPixROI1 = 0, Rect* validPixROI2 = 0)
+ //
+
+ //javadoc: stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, R1, R2, P1, P2, Q, flags, alpha, newImageSize, validPixROI1, validPixROI2)
+ public static void stereoRectify(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat R1, Mat R2, Mat P1, Mat P2, Mat Q, int flags, double alpha, Size newImageSize, Rect validPixROI1, Rect validPixROI2)
+ {
+ double[] validPixROI1_out = new double[4];
+ double[] validPixROI2_out = new double[4];
+ stereoRectify_0(cameraMatrix1.nativeObj, distCoeffs1.nativeObj, cameraMatrix2.nativeObj, distCoeffs2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, R1.nativeObj, R2.nativeObj, P1.nativeObj, P2.nativeObj, Q.nativeObj, flags, alpha, newImageSize.width, newImageSize.height, validPixROI1_out, validPixROI2_out);
+ if(validPixROI1!=null){ validPixROI1.x = (int)validPixROI1_out[0]; validPixROI1.y = (int)validPixROI1_out[1]; validPixROI1.width = (int)validPixROI1_out[2]; validPixROI1.height = (int)validPixROI1_out[3]; }
+ if(validPixROI2!=null){ validPixROI2.x = (int)validPixROI2_out[0]; validPixROI2.y = (int)validPixROI2_out[1]; validPixROI2.width = (int)validPixROI2_out[2]; validPixROI2.height = (int)validPixROI2_out[3]; }
+ return;
+ }
+
+ //javadoc: stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, R1, R2, P1, P2, Q)
+ public static void stereoRectify(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat R1, Mat R2, Mat P1, Mat P2, Mat Q)
+ {
+
+ stereoRectify_1(cameraMatrix1.nativeObj, distCoeffs1.nativeObj, cameraMatrix2.nativeObj, distCoeffs2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, T.nativeObj, R1.nativeObj, R2.nativeObj, P1.nativeObj, P2.nativeObj, Q.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void triangulatePoints(Mat projMatr1, Mat projMatr2, Mat projPoints1, Mat projPoints2, Mat& points4D)
+ //
+
+ //javadoc: triangulatePoints(projMatr1, projMatr2, projPoints1, projPoints2, points4D)
+ public static void triangulatePoints(Mat projMatr1, Mat projMatr2, Mat projPoints1, Mat projPoints2, Mat points4D)
+ {
+
+ triangulatePoints_0(projMatr1.nativeObj, projMatr2.nativeObj, projPoints1.nativeObj, projPoints2.nativeObj, points4D.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void validateDisparity(Mat& disparity, Mat cost, int minDisparity, int numberOfDisparities, int disp12MaxDisp = 1)
+ //
+
+ //javadoc: validateDisparity(disparity, cost, minDisparity, numberOfDisparities, disp12MaxDisp)
+ public static void validateDisparity(Mat disparity, Mat cost, int minDisparity, int numberOfDisparities, int disp12MaxDisp)
+ {
+
+ validateDisparity_0(disparity.nativeObj, cost.nativeObj, minDisparity, numberOfDisparities, disp12MaxDisp);
+
+ return;
+ }
+
+ //javadoc: validateDisparity(disparity, cost, minDisparity, numberOfDisparities)
+ public static void validateDisparity(Mat disparity, Mat cost, int minDisparity, int numberOfDisparities)
+ {
+
+ validateDisparity_1(disparity.nativeObj, cost.nativeObj, minDisparity, numberOfDisparities);
+
+ return;
+ }
+
+
+ //
+ // C++: void distortPoints(Mat undistorted, Mat& distorted, Mat K, Mat D, double alpha = 0)
+ //
+
+ //javadoc: distortPoints(undistorted, distorted, K, D, alpha)
+ public static void distortPoints(Mat undistorted, Mat distorted, Mat K, Mat D, double alpha)
+ {
+
+ distortPoints_0(undistorted.nativeObj, distorted.nativeObj, K.nativeObj, D.nativeObj, alpha);
+
+ return;
+ }
+
+ //javadoc: distortPoints(undistorted, distorted, K, D)
+ public static void distortPoints(Mat undistorted, Mat distorted, Mat K, Mat D)
+ {
+
+ distortPoints_1(undistorted.nativeObj, distorted.nativeObj, K.nativeObj, D.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void estimateNewCameraMatrixForUndistortRectify(Mat K, Mat D, Size image_size, Mat R, Mat& P, double balance = 0.0, Size new_size = Size(), double fov_scale = 1.0)
+ //
+
+ //javadoc: estimateNewCameraMatrixForUndistortRectify(K, D, image_size, R, P, balance, new_size, fov_scale)
+ public static void estimateNewCameraMatrixForUndistortRectify(Mat K, Mat D, Size image_size, Mat R, Mat P, double balance, Size new_size, double fov_scale)
+ {
+
+ estimateNewCameraMatrixForUndistortRectify_0(K.nativeObj, D.nativeObj, image_size.width, image_size.height, R.nativeObj, P.nativeObj, balance, new_size.width, new_size.height, fov_scale);
+
+ return;
+ }
+
+ //javadoc: estimateNewCameraMatrixForUndistortRectify(K, D, image_size, R, P)
+ public static void estimateNewCameraMatrixForUndistortRectify(Mat K, Mat D, Size image_size, Mat R, Mat P)
+ {
+
+ estimateNewCameraMatrixForUndistortRectify_1(K.nativeObj, D.nativeObj, image_size.width, image_size.height, R.nativeObj, P.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void initUndistortRectifyMap(Mat K, Mat D, Mat R, Mat P, Size size, int m1type, Mat& map1, Mat& map2)
+ //
+
+ //javadoc: initUndistortRectifyMap(K, D, R, P, size, m1type, map1, map2)
+ public static void initUndistortRectifyMap(Mat K, Mat D, Mat R, Mat P, Size size, int m1type, Mat map1, Mat map2)
+ {
+
+ initUndistortRectifyMap_0(K.nativeObj, D.nativeObj, R.nativeObj, P.nativeObj, size.width, size.height, m1type, map1.nativeObj, map2.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void projectPoints(vector_Point3f objectPoints, vector_Point2f& imagePoints, Mat rvec, Mat tvec, Mat K, Mat D, double alpha = 0, Mat& jacobian = Mat())
+ //
+
+ //javadoc: projectPoints(objectPoints, imagePoints, rvec, tvec, K, D, alpha, jacobian)
+ public static void projectPoints(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat rvec, Mat tvec, Mat K, Mat D, double alpha, Mat jacobian)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat imagePoints_mat = imagePoints;
+ projectPoints_2(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, rvec.nativeObj, tvec.nativeObj, K.nativeObj, D.nativeObj, alpha, jacobian.nativeObj);
+
+ return;
+ }
+
+ //javadoc: projectPoints(objectPoints, imagePoints, rvec, tvec, K, D)
+ public static void projectPoints(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat rvec, Mat tvec, Mat K, Mat D)
+ {
+ Mat objectPoints_mat = objectPoints;
+ Mat imagePoints_mat = imagePoints;
+ projectPoints_3(objectPoints_mat.nativeObj, imagePoints_mat.nativeObj, rvec.nativeObj, tvec.nativeObj, K.nativeObj, D.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void stereoRectify(Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat tvec, Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q, int flags, Size newImageSize = Size(), double balance = 0.0, double fov_scale = 1.0)
+ //
+
+ //javadoc: stereoRectify(K1, D1, K2, D2, imageSize, R, tvec, R1, R2, P1, P2, Q, flags, newImageSize, balance, fov_scale)
+ public static void stereoRectify(Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat tvec, Mat R1, Mat R2, Mat P1, Mat P2, Mat Q, int flags, Size newImageSize, double balance, double fov_scale)
+ {
+
+ stereoRectify_2(K1.nativeObj, D1.nativeObj, K2.nativeObj, D2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, tvec.nativeObj, R1.nativeObj, R2.nativeObj, P1.nativeObj, P2.nativeObj, Q.nativeObj, flags, newImageSize.width, newImageSize.height, balance, fov_scale);
+
+ return;
+ }
+
+ //javadoc: stereoRectify(K1, D1, K2, D2, imageSize, R, tvec, R1, R2, P1, P2, Q, flags)
+ public static void stereoRectify(Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat tvec, Mat R1, Mat R2, Mat P1, Mat P2, Mat Q, int flags)
+ {
+
+ stereoRectify_3(K1.nativeObj, D1.nativeObj, K2.nativeObj, D2.nativeObj, imageSize.width, imageSize.height, R.nativeObj, tvec.nativeObj, R1.nativeObj, R2.nativeObj, P1.nativeObj, P2.nativeObj, Q.nativeObj, flags);
+
+ return;
+ }
+
+
+ //
+ // C++: void undistortImage(Mat distorted, Mat& undistorted, Mat K, Mat D, Mat Knew = cv::Mat(), Size new_size = Size())
+ //
+
+ //javadoc: undistortImage(distorted, undistorted, K, D, Knew, new_size)
+ public static void undistortImage(Mat distorted, Mat undistorted, Mat K, Mat D, Mat Knew, Size new_size)
+ {
+
+ undistortImage_0(distorted.nativeObj, undistorted.nativeObj, K.nativeObj, D.nativeObj, Knew.nativeObj, new_size.width, new_size.height);
+
+ return;
+ }
+
+ //javadoc: undistortImage(distorted, undistorted, K, D)
+ public static void undistortImage(Mat distorted, Mat undistorted, Mat K, Mat D)
+ {
+
+ undistortImage_1(distorted.nativeObj, undistorted.nativeObj, K.nativeObj, D.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void undistortPoints(Mat distorted, Mat& undistorted, Mat K, Mat D, Mat R = Mat(), Mat P = Mat())
+ //
+
+ //javadoc: undistortPoints(distorted, undistorted, K, D, R, P)
+ public static void undistortPoints(Mat distorted, Mat undistorted, Mat K, Mat D, Mat R, Mat P)
+ {
+
+ undistortPoints_0(distorted.nativeObj, undistorted.nativeObj, K.nativeObj, D.nativeObj, R.nativeObj, P.nativeObj);
+
+ return;
+ }
+
+ //javadoc: undistortPoints(distorted, undistorted, K, D)
+ public static void undistortPoints(Mat distorted, Mat undistorted, Mat K, Mat D)
+ {
+
+ undistortPoints_1(distorted.nativeObj, undistorted.nativeObj, K.nativeObj, D.nativeObj);
+
+ return;
+ }
+
+
+
+
+ // C++: Mat estimateAffine2D(Mat from, Mat to, Mat& inliers = Mat(), int method = RANSAC, double ransacReprojThreshold = 3, size_t maxIters = 2000, double confidence = 0.99, size_t refineIters = 10)
+ private static native long estimateAffine2D_0(long from_nativeObj, long to_nativeObj, long inliers_nativeObj, int method, double ransacReprojThreshold, long maxIters, double confidence, long refineIters);
+ private static native long estimateAffine2D_1(long from_nativeObj, long to_nativeObj);
+
+ // C++: Mat estimateAffinePartial2D(Mat from, Mat to, Mat& inliers = Mat(), int method = RANSAC, double ransacReprojThreshold = 3, size_t maxIters = 2000, double confidence = 0.99, size_t refineIters = 10)
+ private static native long estimateAffinePartial2D_0(long from_nativeObj, long to_nativeObj, long inliers_nativeObj, int method, double ransacReprojThreshold, long maxIters, double confidence, long refineIters);
+ private static native long estimateAffinePartial2D_1(long from_nativeObj, long to_nativeObj);
+
+ // C++: Mat findEssentialMat(Mat points1, Mat points2, Mat cameraMatrix, int method = RANSAC, double prob = 0.999, double threshold = 1.0, Mat& mask = Mat())
+ private static native long findEssentialMat_0(long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj, int method, double prob, double threshold, long mask_nativeObj);
+ private static native long findEssentialMat_1(long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj, int method, double prob, double threshold);
+ private static native long findEssentialMat_2(long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj);
+
+ // C++: Mat findEssentialMat(Mat points1, Mat points2, double focal = 1.0, Point2d pp = Point2d(0, 0), int method = RANSAC, double prob = 0.999, double threshold = 1.0, Mat& mask = Mat())
+ private static native long findEssentialMat_3(long points1_nativeObj, long points2_nativeObj, double focal, double pp_x, double pp_y, int method, double prob, double threshold, long mask_nativeObj);
+ private static native long findEssentialMat_4(long points1_nativeObj, long points2_nativeObj, double focal, double pp_x, double pp_y, int method, double prob, double threshold);
+ private static native long findEssentialMat_5(long points1_nativeObj, long points2_nativeObj);
+
+ // C++: Mat findFundamentalMat(vector_Point2f points1, vector_Point2f points2, int method = FM_RANSAC, double param1 = 3., double param2 = 0.99, Mat& mask = Mat())
+ private static native long findFundamentalMat_0(long points1_mat_nativeObj, long points2_mat_nativeObj, int method, double param1, double param2, long mask_nativeObj);
+ private static native long findFundamentalMat_1(long points1_mat_nativeObj, long points2_mat_nativeObj, int method, double param1, double param2);
+ private static native long findFundamentalMat_2(long points1_mat_nativeObj, long points2_mat_nativeObj);
+
+ // C++: Mat findHomography(vector_Point2f srcPoints, vector_Point2f dstPoints, int method = 0, double ransacReprojThreshold = 3, Mat& mask = Mat(), int maxIters = 2000, double confidence = 0.995)
+ private static native long findHomography_0(long srcPoints_mat_nativeObj, long dstPoints_mat_nativeObj, int method, double ransacReprojThreshold, long mask_nativeObj, int maxIters, double confidence);
+ private static native long findHomography_1(long srcPoints_mat_nativeObj, long dstPoints_mat_nativeObj, int method, double ransacReprojThreshold);
+ private static native long findHomography_2(long srcPoints_mat_nativeObj, long dstPoints_mat_nativeObj);
+
+ // C++: Mat getOptimalNewCameraMatrix(Mat cameraMatrix, Mat distCoeffs, Size imageSize, double alpha, Size newImgSize = Size(), Rect* validPixROI = 0, bool centerPrincipalPoint = false)
+ private static native long getOptimalNewCameraMatrix_0(long cameraMatrix_nativeObj, long distCoeffs_nativeObj, double imageSize_width, double imageSize_height, double alpha, double newImgSize_width, double newImgSize_height, double[] validPixROI_out, boolean centerPrincipalPoint);
+ private static native long getOptimalNewCameraMatrix_1(long cameraMatrix_nativeObj, long distCoeffs_nativeObj, double imageSize_width, double imageSize_height, double alpha);
+
+ // C++: Mat initCameraMatrix2D(vector_vector_Point3f objectPoints, vector_vector_Point2f imagePoints, Size imageSize, double aspectRatio = 1.0)
+ private static native long initCameraMatrix2D_0(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, double aspectRatio);
+ private static native long initCameraMatrix2D_1(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height);
+
+ // C++: Rect getValidDisparityROI(Rect roi1, Rect roi2, int minDisparity, int numberOfDisparities, int SADWindowSize)
+ private static native double[] getValidDisparityROI_0(int roi1_x, int roi1_y, int roi1_width, int roi1_height, int roi2_x, int roi2_y, int roi2_width, int roi2_height, int minDisparity, int numberOfDisparities, int SADWindowSize);
+
+ // C++: Vec3d RQDecomp3x3(Mat src, Mat& mtxR, Mat& mtxQ, Mat& Qx = Mat(), Mat& Qy = Mat(), Mat& Qz = Mat())
+ private static native double[] RQDecomp3x3_0(long src_nativeObj, long mtxR_nativeObj, long mtxQ_nativeObj, long Qx_nativeObj, long Qy_nativeObj, long Qz_nativeObj);
+ private static native double[] RQDecomp3x3_1(long src_nativeObj, long mtxR_nativeObj, long mtxQ_nativeObj);
+
+ // C++: bool findChessboardCorners(Mat image, Size patternSize, vector_Point2f& corners, int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE)
+ private static native boolean findChessboardCorners_0(long image_nativeObj, double patternSize_width, double patternSize_height, long corners_mat_nativeObj, int flags);
+ private static native boolean findChessboardCorners_1(long image_nativeObj, double patternSize_width, double patternSize_height, long corners_mat_nativeObj);
+
+ // C++: bool findCirclesGrid(Mat image, Size patternSize, Mat& centers, int flags = CALIB_CB_SYMMETRIC_GRID, Ptr_FeatureDetector blobDetector = SimpleBlobDetector::create())
+ private static native boolean findCirclesGrid_0(long image_nativeObj, double patternSize_width, double patternSize_height, long centers_nativeObj, int flags);
+ private static native boolean findCirclesGrid_1(long image_nativeObj, double patternSize_width, double patternSize_height, long centers_nativeObj);
+
+ // C++: bool solvePnP(vector_Point3f objectPoints, vector_Point2f imagePoints, Mat cameraMatrix, vector_double distCoeffs, Mat& rvec, Mat& tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE)
+ private static native boolean solvePnP_0(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj, boolean useExtrinsicGuess, int flags);
+ private static native boolean solvePnP_1(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj);
+
+ // C++: bool solvePnPRansac(vector_Point3f objectPoints, vector_Point2f imagePoints, Mat cameraMatrix, vector_double distCoeffs, Mat& rvec, Mat& tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, double confidence = 0.99, Mat& inliers = Mat(), int flags = SOLVEPNP_ITERATIVE)
+ private static native boolean solvePnPRansac_0(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj, boolean useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, long inliers_nativeObj, int flags);
+ private static native boolean solvePnPRansac_1(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj);
+
+ // C++: bool stereoRectifyUncalibrated(Mat points1, Mat points2, Mat F, Size imgSize, Mat& H1, Mat& H2, double threshold = 5)
+ private static native boolean stereoRectifyUncalibrated_0(long points1_nativeObj, long points2_nativeObj, long F_nativeObj, double imgSize_width, double imgSize_height, long H1_nativeObj, long H2_nativeObj, double threshold);
+ private static native boolean stereoRectifyUncalibrated_1(long points1_nativeObj, long points2_nativeObj, long F_nativeObj, double imgSize_width, double imgSize_height, long H1_nativeObj, long H2_nativeObj);
+
+ // C++: double calibrateCamera(vector_Mat objectPoints, vector_Mat imagePoints, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, vector_Mat& rvecs, vector_Mat& tvecs, Mat& stdDeviationsIntrinsics, Mat& stdDeviationsExtrinsics, Mat& perViewErrors, int flags = 0, TermCriteria criteria = TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON))
+ private static native double calibrateCameraExtended_0(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, long stdDeviationsIntrinsics_nativeObj, long stdDeviationsExtrinsics_nativeObj, long perViewErrors_nativeObj, int flags, int criteria_type, int criteria_maxCount, double criteria_epsilon);
+ private static native double calibrateCameraExtended_1(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, long stdDeviationsIntrinsics_nativeObj, long stdDeviationsExtrinsics_nativeObj, long perViewErrors_nativeObj, int flags);
+ private static native double calibrateCameraExtended_2(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, long stdDeviationsIntrinsics_nativeObj, long stdDeviationsExtrinsics_nativeObj, long perViewErrors_nativeObj);
+
+ // C++: double calibrateCamera(vector_Mat objectPoints, vector_Mat imagePoints, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs, vector_Mat& rvecs, vector_Mat& tvecs, int flags = 0, TermCriteria criteria = TermCriteria( TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON))
+ private static native double calibrateCamera_0(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, int flags, int criteria_type, int criteria_maxCount, double criteria_epsilon);
+ private static native double calibrateCamera_1(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, int flags);
+ private static native double calibrateCamera_2(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double imageSize_width, double imageSize_height, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj);
+
+ // C++: double sampsonDistance(Mat pt1, Mat pt2, Mat F)
+ private static native double sampsonDistance_0(long pt1_nativeObj, long pt2_nativeObj, long F_nativeObj);
+
+ // C++: double stereoCalibrate(vector_Mat objectPoints, vector_Mat imagePoints1, vector_Mat imagePoints2, Mat& cameraMatrix1, Mat& distCoeffs1, Mat& cameraMatrix2, Mat& distCoeffs2, Size imageSize, Mat& R, Mat& T, Mat& E, Mat& F, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6))
+ private static native double stereoCalibrate_0(long objectPoints_mat_nativeObj, long imagePoints1_mat_nativeObj, long imagePoints2_mat_nativeObj, long cameraMatrix1_nativeObj, long distCoeffs1_nativeObj, long cameraMatrix2_nativeObj, long distCoeffs2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, long E_nativeObj, long F_nativeObj, int flags, int criteria_type, int criteria_maxCount, double criteria_epsilon);
+ private static native double stereoCalibrate_1(long objectPoints_mat_nativeObj, long imagePoints1_mat_nativeObj, long imagePoints2_mat_nativeObj, long cameraMatrix1_nativeObj, long distCoeffs1_nativeObj, long cameraMatrix2_nativeObj, long distCoeffs2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, long E_nativeObj, long F_nativeObj, int flags);
+ private static native double stereoCalibrate_2(long objectPoints_mat_nativeObj, long imagePoints1_mat_nativeObj, long imagePoints2_mat_nativeObj, long cameraMatrix1_nativeObj, long distCoeffs1_nativeObj, long cameraMatrix2_nativeObj, long distCoeffs2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, long E_nativeObj, long F_nativeObj);
+
+ // C++: double calibrate(vector_Mat objectPoints, vector_Mat imagePoints, Size image_size, Mat& K, Mat& D, vector_Mat& rvecs, vector_Mat& tvecs, int flags = 0, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON))
+ private static native double calibrate_0(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double image_size_width, double image_size_height, long K_nativeObj, long D_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, int flags, int criteria_type, int criteria_maxCount, double criteria_epsilon);
+ private static native double calibrate_1(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double image_size_width, double image_size_height, long K_nativeObj, long D_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, int flags);
+ private static native double calibrate_2(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, double image_size_width, double image_size_height, long K_nativeObj, long D_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj);
+
+ // C++: double stereoCalibrate(vector_Mat objectPoints, vector_Mat imagePoints1, vector_Mat imagePoints2, Mat& K1, Mat& D1, Mat& K2, Mat& D2, Size imageSize, Mat& R, Mat& T, int flags = fisheye::CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON))
+ private static native double stereoCalibrate_3(long objectPoints_mat_nativeObj, long imagePoints1_mat_nativeObj, long imagePoints2_mat_nativeObj, long K1_nativeObj, long D1_nativeObj, long K2_nativeObj, long D2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, int flags, int criteria_type, int criteria_maxCount, double criteria_epsilon);
+ private static native double stereoCalibrate_4(long objectPoints_mat_nativeObj, long imagePoints1_mat_nativeObj, long imagePoints2_mat_nativeObj, long K1_nativeObj, long D1_nativeObj, long K2_nativeObj, long D2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, int flags);
+ private static native double stereoCalibrate_5(long objectPoints_mat_nativeObj, long imagePoints1_mat_nativeObj, long imagePoints2_mat_nativeObj, long K1_nativeObj, long D1_nativeObj, long K2_nativeObj, long D2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj);
+
+ // C++: float rectify3Collinear(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Mat cameraMatrix3, Mat distCoeffs3, vector_Mat imgpt1, vector_Mat imgpt3, Size imageSize, Mat R12, Mat T12, Mat R13, Mat T13, Mat& R1, Mat& R2, Mat& R3, Mat& P1, Mat& P2, Mat& P3, Mat& Q, double alpha, Size newImgSize, Rect* roi1, Rect* roi2, int flags)
+ private static native float rectify3Collinear_0(long cameraMatrix1_nativeObj, long distCoeffs1_nativeObj, long cameraMatrix2_nativeObj, long distCoeffs2_nativeObj, long cameraMatrix3_nativeObj, long distCoeffs3_nativeObj, long imgpt1_mat_nativeObj, long imgpt3_mat_nativeObj, double imageSize_width, double imageSize_height, long R12_nativeObj, long T12_nativeObj, long R13_nativeObj, long T13_nativeObj, long R1_nativeObj, long R2_nativeObj, long R3_nativeObj, long P1_nativeObj, long P2_nativeObj, long P3_nativeObj, long Q_nativeObj, double alpha, double newImgSize_width, double newImgSize_height, double[] roi1_out, double[] roi2_out, int flags);
+
+ // C++: int decomposeHomographyMat(Mat H, Mat K, vector_Mat& rotations, vector_Mat& translations, vector_Mat& normals)
+ private static native int decomposeHomographyMat_0(long H_nativeObj, long K_nativeObj, long rotations_mat_nativeObj, long translations_mat_nativeObj, long normals_mat_nativeObj);
+
+ // C++: int estimateAffine3D(Mat src, Mat dst, Mat& out, Mat& inliers, double ransacThreshold = 3, double confidence = 0.99)
+ private static native int estimateAffine3D_0(long src_nativeObj, long dst_nativeObj, long out_nativeObj, long inliers_nativeObj, double ransacThreshold, double confidence);
+ private static native int estimateAffine3D_1(long src_nativeObj, long dst_nativeObj, long out_nativeObj, long inliers_nativeObj);
+
+ // C++: int recoverPose(Mat E, Mat points1, Mat points2, Mat& R, Mat& t, double focal = 1.0, Point2d pp = Point2d(0, 0), Mat& mask = Mat())
+ private static native int recoverPose_0(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long R_nativeObj, long t_nativeObj, double focal, double pp_x, double pp_y, long mask_nativeObj);
+ private static native int recoverPose_1(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long R_nativeObj, long t_nativeObj, double focal, double pp_x, double pp_y);
+ private static native int recoverPose_2(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long R_nativeObj, long t_nativeObj);
+
+ // C++: int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat& R, Mat& t, Mat& mask = Mat())
+ private static native int recoverPose_3(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj, long R_nativeObj, long t_nativeObj, long mask_nativeObj);
+ private static native int recoverPose_4(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj, long R_nativeObj, long t_nativeObj);
+
+ // C++: int recoverPose(Mat E, Mat points1, Mat points2, Mat cameraMatrix, Mat& R, Mat& t, double distanceThresh, Mat& mask = Mat(), Mat& triangulatedPoints = Mat())
+ private static native int recoverPose_5(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj, long R_nativeObj, long t_nativeObj, double distanceThresh, long mask_nativeObj, long triangulatedPoints_nativeObj);
+ private static native int recoverPose_6(long E_nativeObj, long points1_nativeObj, long points2_nativeObj, long cameraMatrix_nativeObj, long R_nativeObj, long t_nativeObj, double distanceThresh);
+
+ // C++: int solveP3P(Mat objectPoints, Mat imagePoints, Mat cameraMatrix, Mat distCoeffs, vector_Mat& rvecs, vector_Mat& tvecs, int flags)
+ private static native int solveP3P_0(long objectPoints_nativeObj, long imagePoints_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_nativeObj, long rvecs_mat_nativeObj, long tvecs_mat_nativeObj, int flags);
+
+ // C++: void Rodrigues(Mat src, Mat& dst, Mat& jacobian = Mat())
+ private static native void Rodrigues_0(long src_nativeObj, long dst_nativeObj, long jacobian_nativeObj);
+ private static native void Rodrigues_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void calibrationMatrixValues(Mat cameraMatrix, Size imageSize, double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength, Point2d& principalPoint, double& aspectRatio)
+ private static native void calibrationMatrixValues_0(long cameraMatrix_nativeObj, double imageSize_width, double imageSize_height, double apertureWidth, double apertureHeight, double[] fovx_out, double[] fovy_out, double[] focalLength_out, double[] principalPoint_out, double[] aspectRatio_out);
+
+ // C++: void composeRT(Mat rvec1, Mat tvec1, Mat rvec2, Mat tvec2, Mat& rvec3, Mat& tvec3, Mat& dr3dr1 = Mat(), Mat& dr3dt1 = Mat(), Mat& dr3dr2 = Mat(), Mat& dr3dt2 = Mat(), Mat& dt3dr1 = Mat(), Mat& dt3dt1 = Mat(), Mat& dt3dr2 = Mat(), Mat& dt3dt2 = Mat())
+ private static native void composeRT_0(long rvec1_nativeObj, long tvec1_nativeObj, long rvec2_nativeObj, long tvec2_nativeObj, long rvec3_nativeObj, long tvec3_nativeObj, long dr3dr1_nativeObj, long dr3dt1_nativeObj, long dr3dr2_nativeObj, long dr3dt2_nativeObj, long dt3dr1_nativeObj, long dt3dt1_nativeObj, long dt3dr2_nativeObj, long dt3dt2_nativeObj);
+ private static native void composeRT_1(long rvec1_nativeObj, long tvec1_nativeObj, long rvec2_nativeObj, long tvec2_nativeObj, long rvec3_nativeObj, long tvec3_nativeObj);
+
+ // C++: void computeCorrespondEpilines(Mat points, int whichImage, Mat F, Mat& lines)
+ private static native void computeCorrespondEpilines_0(long points_nativeObj, int whichImage, long F_nativeObj, long lines_nativeObj);
+
+ // C++: void convertPointsFromHomogeneous(Mat src, Mat& dst)
+ private static native void convertPointsFromHomogeneous_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void convertPointsToHomogeneous(Mat src, Mat& dst)
+ private static native void convertPointsToHomogeneous_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void correctMatches(Mat F, Mat points1, Mat points2, Mat& newPoints1, Mat& newPoints2)
+ private static native void correctMatches_0(long F_nativeObj, long points1_nativeObj, long points2_nativeObj, long newPoints1_nativeObj, long newPoints2_nativeObj);
+
+ // C++: void decomposeEssentialMat(Mat E, Mat& R1, Mat& R2, Mat& t)
+ private static native void decomposeEssentialMat_0(long E_nativeObj, long R1_nativeObj, long R2_nativeObj, long t_nativeObj);
+
+ // C++: void decomposeProjectionMatrix(Mat projMatrix, Mat& cameraMatrix, Mat& rotMatrix, Mat& transVect, Mat& rotMatrixX = Mat(), Mat& rotMatrixY = Mat(), Mat& rotMatrixZ = Mat(), Mat& eulerAngles = Mat())
+ private static native void decomposeProjectionMatrix_0(long projMatrix_nativeObj, long cameraMatrix_nativeObj, long rotMatrix_nativeObj, long transVect_nativeObj, long rotMatrixX_nativeObj, long rotMatrixY_nativeObj, long rotMatrixZ_nativeObj, long eulerAngles_nativeObj);
+ private static native void decomposeProjectionMatrix_1(long projMatrix_nativeObj, long cameraMatrix_nativeObj, long rotMatrix_nativeObj, long transVect_nativeObj);
+
+ // C++: void drawChessboardCorners(Mat& image, Size patternSize, vector_Point2f corners, bool patternWasFound)
+ private static native void drawChessboardCorners_0(long image_nativeObj, double patternSize_width, double patternSize_height, long corners_mat_nativeObj, boolean patternWasFound);
+
+ // C++: void filterSpeckles(Mat& img, double newVal, int maxSpeckleSize, double maxDiff, Mat& buf = Mat())
+ private static native void filterSpeckles_0(long img_nativeObj, double newVal, int maxSpeckleSize, double maxDiff, long buf_nativeObj);
+ private static native void filterSpeckles_1(long img_nativeObj, double newVal, int maxSpeckleSize, double maxDiff);
+
+ // C++: void matMulDeriv(Mat A, Mat B, Mat& dABdA, Mat& dABdB)
+ private static native void matMulDeriv_0(long A_nativeObj, long B_nativeObj, long dABdA_nativeObj, long dABdB_nativeObj);
+
+ // C++: void projectPoints(vector_Point3f objectPoints, Mat rvec, Mat tvec, Mat cameraMatrix, vector_double distCoeffs, vector_Point2f& imagePoints, Mat& jacobian = Mat(), double aspectRatio = 0)
+ private static native void projectPoints_0(long objectPoints_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_mat_nativeObj, long imagePoints_mat_nativeObj, long jacobian_nativeObj, double aspectRatio);
+ private static native void projectPoints_1(long objectPoints_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj, long cameraMatrix_nativeObj, long distCoeffs_mat_nativeObj, long imagePoints_mat_nativeObj);
+
+ // C++: void reprojectImageTo3D(Mat disparity, Mat& _3dImage, Mat Q, bool handleMissingValues = false, int ddepth = -1)
+ private static native void reprojectImageTo3D_0(long disparity_nativeObj, long _3dImage_nativeObj, long Q_nativeObj, boolean handleMissingValues, int ddepth);
+ private static native void reprojectImageTo3D_1(long disparity_nativeObj, long _3dImage_nativeObj, long Q_nativeObj, boolean handleMissingValues);
+ private static native void reprojectImageTo3D_2(long disparity_nativeObj, long _3dImage_nativeObj, long Q_nativeObj);
+
+ // C++: void stereoRectify(Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, Mat R, Mat T, Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q, int flags = CALIB_ZERO_DISPARITY, double alpha = -1, Size newImageSize = Size(), Rect* validPixROI1 = 0, Rect* validPixROI2 = 0)
+ private static native void stereoRectify_0(long cameraMatrix1_nativeObj, long distCoeffs1_nativeObj, long cameraMatrix2_nativeObj, long distCoeffs2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, long R1_nativeObj, long R2_nativeObj, long P1_nativeObj, long P2_nativeObj, long Q_nativeObj, int flags, double alpha, double newImageSize_width, double newImageSize_height, double[] validPixROI1_out, double[] validPixROI2_out);
+ private static native void stereoRectify_1(long cameraMatrix1_nativeObj, long distCoeffs1_nativeObj, long cameraMatrix2_nativeObj, long distCoeffs2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long T_nativeObj, long R1_nativeObj, long R2_nativeObj, long P1_nativeObj, long P2_nativeObj, long Q_nativeObj);
+
+ // C++: void triangulatePoints(Mat projMatr1, Mat projMatr2, Mat projPoints1, Mat projPoints2, Mat& points4D)
+ private static native void triangulatePoints_0(long projMatr1_nativeObj, long projMatr2_nativeObj, long projPoints1_nativeObj, long projPoints2_nativeObj, long points4D_nativeObj);
+
+ // C++: void validateDisparity(Mat& disparity, Mat cost, int minDisparity, int numberOfDisparities, int disp12MaxDisp = 1)
+ private static native void validateDisparity_0(long disparity_nativeObj, long cost_nativeObj, int minDisparity, int numberOfDisparities, int disp12MaxDisp);
+ private static native void validateDisparity_1(long disparity_nativeObj, long cost_nativeObj, int minDisparity, int numberOfDisparities);
+
+ // C++: void distortPoints(Mat undistorted, Mat& distorted, Mat K, Mat D, double alpha = 0)
+ private static native void distortPoints_0(long undistorted_nativeObj, long distorted_nativeObj, long K_nativeObj, long D_nativeObj, double alpha);
+ private static native void distortPoints_1(long undistorted_nativeObj, long distorted_nativeObj, long K_nativeObj, long D_nativeObj);
+
+ // C++: void estimateNewCameraMatrixForUndistortRectify(Mat K, Mat D, Size image_size, Mat R, Mat& P, double balance = 0.0, Size new_size = Size(), double fov_scale = 1.0)
+ private static native void estimateNewCameraMatrixForUndistortRectify_0(long K_nativeObj, long D_nativeObj, double image_size_width, double image_size_height, long R_nativeObj, long P_nativeObj, double balance, double new_size_width, double new_size_height, double fov_scale);
+ private static native void estimateNewCameraMatrixForUndistortRectify_1(long K_nativeObj, long D_nativeObj, double image_size_width, double image_size_height, long R_nativeObj, long P_nativeObj);
+
+ // C++: void initUndistortRectifyMap(Mat K, Mat D, Mat R, Mat P, Size size, int m1type, Mat& map1, Mat& map2)
+ private static native void initUndistortRectifyMap_0(long K_nativeObj, long D_nativeObj, long R_nativeObj, long P_nativeObj, double size_width, double size_height, int m1type, long map1_nativeObj, long map2_nativeObj);
+
+ // C++: void projectPoints(vector_Point3f objectPoints, vector_Point2f& imagePoints, Mat rvec, Mat tvec, Mat K, Mat D, double alpha = 0, Mat& jacobian = Mat())
+ private static native void projectPoints_2(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj, long K_nativeObj, long D_nativeObj, double alpha, long jacobian_nativeObj);
+ private static native void projectPoints_3(long objectPoints_mat_nativeObj, long imagePoints_mat_nativeObj, long rvec_nativeObj, long tvec_nativeObj, long K_nativeObj, long D_nativeObj);
+
+ // C++: void stereoRectify(Mat K1, Mat D1, Mat K2, Mat D2, Size imageSize, Mat R, Mat tvec, Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q, int flags, Size newImageSize = Size(), double balance = 0.0, double fov_scale = 1.0)
+ private static native void stereoRectify_2(long K1_nativeObj, long D1_nativeObj, long K2_nativeObj, long D2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long tvec_nativeObj, long R1_nativeObj, long R2_nativeObj, long P1_nativeObj, long P2_nativeObj, long Q_nativeObj, int flags, double newImageSize_width, double newImageSize_height, double balance, double fov_scale);
+ private static native void stereoRectify_3(long K1_nativeObj, long D1_nativeObj, long K2_nativeObj, long D2_nativeObj, double imageSize_width, double imageSize_height, long R_nativeObj, long tvec_nativeObj, long R1_nativeObj, long R2_nativeObj, long P1_nativeObj, long P2_nativeObj, long Q_nativeObj, int flags);
+
+ // C++: void undistortImage(Mat distorted, Mat& undistorted, Mat K, Mat D, Mat Knew = cv::Mat(), Size new_size = Size())
+ private static native void undistortImage_0(long distorted_nativeObj, long undistorted_nativeObj, long K_nativeObj, long D_nativeObj, long Knew_nativeObj, double new_size_width, double new_size_height);
+ private static native void undistortImage_1(long distorted_nativeObj, long undistorted_nativeObj, long K_nativeObj, long D_nativeObj);
+
+ // C++: void undistortPoints(Mat distorted, Mat& undistorted, Mat K, Mat D, Mat R = Mat(), Mat P = Mat())
+ private static native void undistortPoints_0(long distorted_nativeObj, long undistorted_nativeObj, long K_nativeObj, long D_nativeObj, long R_nativeObj, long P_nativeObj);
+ private static native void undistortPoints_1(long distorted_nativeObj, long undistorted_nativeObj, long K_nativeObj, long D_nativeObj);
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoBM.java b/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoBM.java
new file mode 100644
index 00000000000..18aba05c01d
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoBM.java
@@ -0,0 +1,330 @@
+
+//
+// This file is auto-generated. Please don't modify it!
+//
+package org.opencv.calib3d;
+
+import org.opencv.core.Rect;
+
+// C++: class StereoBM
+//javadoc: StereoBM
+public class StereoBM extends StereoMatcher {
+
+ protected StereoBM(long addr) { super(addr); }
+
+
+ public static final int
+ PREFILTER_NORMALIZED_RESPONSE = 0,
+ PREFILTER_XSOBEL = 1;
+
+
+ //
+ // C++: static Ptr_StereoBM create(int numDisparities = 0, int blockSize = 21)
+ //
+
+ //javadoc: StereoBM::create(numDisparities, blockSize)
+ public static StereoBM create(int numDisparities, int blockSize)
+ {
+
+ StereoBM retVal = new StereoBM(create_0(numDisparities, blockSize));
+
+ return retVal;
+ }
+
+ //javadoc: StereoBM::create()
+ public static StereoBM create()
+ {
+
+ StereoBM retVal = new StereoBM(create_1());
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Rect getROI1()
+ //
+
+ //javadoc: StereoBM::getROI1()
+ public Rect getROI1()
+ {
+
+ Rect retVal = new Rect(getROI1_0(nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Rect getROI2()
+ //
+
+ //javadoc: StereoBM::getROI2()
+ public Rect getROI2()
+ {
+
+ Rect retVal = new Rect(getROI2_0(nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getPreFilterCap()
+ //
+
+ //javadoc: StereoBM::getPreFilterCap()
+ public int getPreFilterCap()
+ {
+
+ int retVal = getPreFilterCap_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getPreFilterSize()
+ //
+
+ //javadoc: StereoBM::getPreFilterSize()
+ public int getPreFilterSize()
+ {
+
+ int retVal = getPreFilterSize_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getPreFilterType()
+ //
+
+ //javadoc: StereoBM::getPreFilterType()
+ public int getPreFilterType()
+ {
+
+ int retVal = getPreFilterType_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getSmallerBlockSize()
+ //
+
+ //javadoc: StereoBM::getSmallerBlockSize()
+ public int getSmallerBlockSize()
+ {
+
+ int retVal = getSmallerBlockSize_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getTextureThreshold()
+ //
+
+ //javadoc: StereoBM::getTextureThreshold()
+ public int getTextureThreshold()
+ {
+
+ int retVal = getTextureThreshold_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getUniquenessRatio()
+ //
+
+ //javadoc: StereoBM::getUniquenessRatio()
+ public int getUniquenessRatio()
+ {
+
+ int retVal = getUniquenessRatio_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: void setPreFilterCap(int preFilterCap)
+ //
+
+ //javadoc: StereoBM::setPreFilterCap(preFilterCap)
+ public void setPreFilterCap(int preFilterCap)
+ {
+
+ setPreFilterCap_0(nativeObj, preFilterCap);
+
+ return;
+ }
+
+
+ //
+ // C++: void setPreFilterSize(int preFilterSize)
+ //
+
+ //javadoc: StereoBM::setPreFilterSize(preFilterSize)
+ public void setPreFilterSize(int preFilterSize)
+ {
+
+ setPreFilterSize_0(nativeObj, preFilterSize);
+
+ return;
+ }
+
+
+ //
+ // C++: void setPreFilterType(int preFilterType)
+ //
+
+ //javadoc: StereoBM::setPreFilterType(preFilterType)
+ public void setPreFilterType(int preFilterType)
+ {
+
+ setPreFilterType_0(nativeObj, preFilterType);
+
+ return;
+ }
+
+
+ //
+ // C++: void setROI1(Rect roi1)
+ //
+
+ //javadoc: StereoBM::setROI1(roi1)
+ public void setROI1(Rect roi1)
+ {
+
+ setROI1_0(nativeObj, roi1.x, roi1.y, roi1.width, roi1.height);
+
+ return;
+ }
+
+
+ //
+ // C++: void setROI2(Rect roi2)
+ //
+
+ //javadoc: StereoBM::setROI2(roi2)
+ public void setROI2(Rect roi2)
+ {
+
+ setROI2_0(nativeObj, roi2.x, roi2.y, roi2.width, roi2.height);
+
+ return;
+ }
+
+
+ //
+ // C++: void setSmallerBlockSize(int blockSize)
+ //
+
+ //javadoc: StereoBM::setSmallerBlockSize(blockSize)
+ public void setSmallerBlockSize(int blockSize)
+ {
+
+ setSmallerBlockSize_0(nativeObj, blockSize);
+
+ return;
+ }
+
+
+ //
+ // C++: void setTextureThreshold(int textureThreshold)
+ //
+
+ //javadoc: StereoBM::setTextureThreshold(textureThreshold)
+ public void setTextureThreshold(int textureThreshold)
+ {
+
+ setTextureThreshold_0(nativeObj, textureThreshold);
+
+ return;
+ }
+
+
+ //
+ // C++: void setUniquenessRatio(int uniquenessRatio)
+ //
+
+ //javadoc: StereoBM::setUniquenessRatio(uniquenessRatio)
+ public void setUniquenessRatio(int uniquenessRatio)
+ {
+
+ setUniquenessRatio_0(nativeObj, uniquenessRatio);
+
+ return;
+ }
+
+
+ @Override
+ protected void finalize() throws Throwable {
+ delete(nativeObj);
+ }
+
+
+
+ // C++: static Ptr_StereoBM create(int numDisparities = 0, int blockSize = 21)
+ private static native long create_0(int numDisparities, int blockSize);
+ private static native long create_1();
+
+ // C++: Rect getROI1()
+ private static native double[] getROI1_0(long nativeObj);
+
+ // C++: Rect getROI2()
+ private static native double[] getROI2_0(long nativeObj);
+
+ // C++: int getPreFilterCap()
+ private static native int getPreFilterCap_0(long nativeObj);
+
+ // C++: int getPreFilterSize()
+ private static native int getPreFilterSize_0(long nativeObj);
+
+ // C++: int getPreFilterType()
+ private static native int getPreFilterType_0(long nativeObj);
+
+ // C++: int getSmallerBlockSize()
+ private static native int getSmallerBlockSize_0(long nativeObj);
+
+ // C++: int getTextureThreshold()
+ private static native int getTextureThreshold_0(long nativeObj);
+
+ // C++: int getUniquenessRatio()
+ private static native int getUniquenessRatio_0(long nativeObj);
+
+ // C++: void setPreFilterCap(int preFilterCap)
+ private static native void setPreFilterCap_0(long nativeObj, int preFilterCap);
+
+ // C++: void setPreFilterSize(int preFilterSize)
+ private static native void setPreFilterSize_0(long nativeObj, int preFilterSize);
+
+ // C++: void setPreFilterType(int preFilterType)
+ private static native void setPreFilterType_0(long nativeObj, int preFilterType);
+
+ // C++: void setROI1(Rect roi1)
+ private static native void setROI1_0(long nativeObj, int roi1_x, int roi1_y, int roi1_width, int roi1_height);
+
+ // C++: void setROI2(Rect roi2)
+ private static native void setROI2_0(long nativeObj, int roi2_x, int roi2_y, int roi2_width, int roi2_height);
+
+ // C++: void setSmallerBlockSize(int blockSize)
+ private static native void setSmallerBlockSize_0(long nativeObj, int blockSize);
+
+ // C++: void setTextureThreshold(int textureThreshold)
+ private static native void setTextureThreshold_0(long nativeObj, int textureThreshold);
+
+ // C++: void setUniquenessRatio(int uniquenessRatio)
+ private static native void setUniquenessRatio_0(long nativeObj, int uniquenessRatio);
+
+ // native support for java finalize()
+ private static native void delete(long nativeObj);
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoMatcher.java b/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoMatcher.java
new file mode 100644
index 00000000000..6b3c4f63fd0
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoMatcher.java
@@ -0,0 +1,253 @@
+
+//
+// This file is auto-generated. Please don't modify it!
+//
+package org.opencv.calib3d;
+
+import org.opencv.core.Algorithm;
+import org.opencv.core.Mat;
+
+// C++: class StereoMatcher
+//javadoc: StereoMatcher
+public class StereoMatcher extends Algorithm {
+
+ protected StereoMatcher(long addr) { super(addr); }
+
+
+ public static final int
+ DISP_SHIFT = 4,
+ DISP_SCALE = (1 << DISP_SHIFT);
+
+
+ //
+ // C++: int getBlockSize()
+ //
+
+ //javadoc: StereoMatcher::getBlockSize()
+ public int getBlockSize()
+ {
+
+ int retVal = getBlockSize_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getDisp12MaxDiff()
+ //
+
+ //javadoc: StereoMatcher::getDisp12MaxDiff()
+ public int getDisp12MaxDiff()
+ {
+
+ int retVal = getDisp12MaxDiff_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getMinDisparity()
+ //
+
+ //javadoc: StereoMatcher::getMinDisparity()
+ public int getMinDisparity()
+ {
+
+ int retVal = getMinDisparity_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getNumDisparities()
+ //
+
+ //javadoc: StereoMatcher::getNumDisparities()
+ public int getNumDisparities()
+ {
+
+ int retVal = getNumDisparities_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getSpeckleRange()
+ //
+
+ //javadoc: StereoMatcher::getSpeckleRange()
+ public int getSpeckleRange()
+ {
+
+ int retVal = getSpeckleRange_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getSpeckleWindowSize()
+ //
+
+ //javadoc: StereoMatcher::getSpeckleWindowSize()
+ public int getSpeckleWindowSize()
+ {
+
+ int retVal = getSpeckleWindowSize_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: void compute(Mat left, Mat right, Mat& disparity)
+ //
+
+ //javadoc: StereoMatcher::compute(left, right, disparity)
+ public void compute(Mat left, Mat right, Mat disparity)
+ {
+
+ compute_0(nativeObj, left.nativeObj, right.nativeObj, disparity.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void setBlockSize(int blockSize)
+ //
+
+ //javadoc: StereoMatcher::setBlockSize(blockSize)
+ public void setBlockSize(int blockSize)
+ {
+
+ setBlockSize_0(nativeObj, blockSize);
+
+ return;
+ }
+
+
+ //
+ // C++: void setDisp12MaxDiff(int disp12MaxDiff)
+ //
+
+ //javadoc: StereoMatcher::setDisp12MaxDiff(disp12MaxDiff)
+ public void setDisp12MaxDiff(int disp12MaxDiff)
+ {
+
+ setDisp12MaxDiff_0(nativeObj, disp12MaxDiff);
+
+ return;
+ }
+
+
+ //
+ // C++: void setMinDisparity(int minDisparity)
+ //
+
+ //javadoc: StereoMatcher::setMinDisparity(minDisparity)
+ public void setMinDisparity(int minDisparity)
+ {
+
+ setMinDisparity_0(nativeObj, minDisparity);
+
+ return;
+ }
+
+
+ //
+ // C++: void setNumDisparities(int numDisparities)
+ //
+
+ //javadoc: StereoMatcher::setNumDisparities(numDisparities)
+ public void setNumDisparities(int numDisparities)
+ {
+
+ setNumDisparities_0(nativeObj, numDisparities);
+
+ return;
+ }
+
+
+ //
+ // C++: void setSpeckleRange(int speckleRange)
+ //
+
+ //javadoc: StereoMatcher::setSpeckleRange(speckleRange)
+ public void setSpeckleRange(int speckleRange)
+ {
+
+ setSpeckleRange_0(nativeObj, speckleRange);
+
+ return;
+ }
+
+
+ //
+ // C++: void setSpeckleWindowSize(int speckleWindowSize)
+ //
+
+ //javadoc: StereoMatcher::setSpeckleWindowSize(speckleWindowSize)
+ public void setSpeckleWindowSize(int speckleWindowSize)
+ {
+
+ setSpeckleWindowSize_0(nativeObj, speckleWindowSize);
+
+ return;
+ }
+
+
+ @Override
+ protected void finalize() throws Throwable {
+ delete(nativeObj);
+ }
+
+
+
+ // C++: int getBlockSize()
+ private static native int getBlockSize_0(long nativeObj);
+
+ // C++: int getDisp12MaxDiff()
+ private static native int getDisp12MaxDiff_0(long nativeObj);
+
+ // C++: int getMinDisparity()
+ private static native int getMinDisparity_0(long nativeObj);
+
+ // C++: int getNumDisparities()
+ private static native int getNumDisparities_0(long nativeObj);
+
+ // C++: int getSpeckleRange()
+ private static native int getSpeckleRange_0(long nativeObj);
+
+ // C++: int getSpeckleWindowSize()
+ private static native int getSpeckleWindowSize_0(long nativeObj);
+
+ // C++: void compute(Mat left, Mat right, Mat& disparity)
+ private static native void compute_0(long nativeObj, long left_nativeObj, long right_nativeObj, long disparity_nativeObj);
+
+ // C++: void setBlockSize(int blockSize)
+ private static native void setBlockSize_0(long nativeObj, int blockSize);
+
+ // C++: void setDisp12MaxDiff(int disp12MaxDiff)
+ private static native void setDisp12MaxDiff_0(long nativeObj, int disp12MaxDiff);
+
+ // C++: void setMinDisparity(int minDisparity)
+ private static native void setMinDisparity_0(long nativeObj, int minDisparity);
+
+ // C++: void setNumDisparities(int numDisparities)
+ private static native void setNumDisparities_0(long nativeObj, int numDisparities);
+
+ // C++: void setSpeckleRange(int speckleRange)
+ private static native void setSpeckleRange_0(long nativeObj, int speckleRange);
+
+ // C++: void setSpeckleWindowSize(int speckleWindowSize)
+ private static native void setSpeckleWindowSize_0(long nativeObj, int speckleWindowSize);
+
+ // native support for java finalize()
+ private static native void delete(long nativeObj);
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoSGBM.java b/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoSGBM.java
new file mode 100644
index 00000000000..d1d7b598b36
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/calib3d/StereoSGBM.java
@@ -0,0 +1,230 @@
+
+//
+// This file is auto-generated. Please don't modify it!
+//
+package org.opencv.calib3d;
+
+
+
+// C++: class StereoSGBM
+//javadoc: StereoSGBM
+public class StereoSGBM extends StereoMatcher {
+
+ protected StereoSGBM(long addr) { super(addr); }
+
+
+ public static final int
+ MODE_SGBM = 0,
+ MODE_HH = 1,
+ MODE_SGBM_3WAY = 2,
+ MODE_HH4 = 3;
+
+
+ //
+ // C++: static Ptr_StereoSGBM create(int minDisparity = 0, int numDisparities = 16, int blockSize = 3, int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, int preFilterCap = 0, int uniquenessRatio = 0, int speckleWindowSize = 0, int speckleRange = 0, int mode = StereoSGBM::MODE_SGBM)
+ //
+
+ //javadoc: StereoSGBM::create(minDisparity, numDisparities, blockSize, P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize, speckleRange, mode)
+ public static StereoSGBM create(int minDisparity, int numDisparities, int blockSize, int P1, int P2, int disp12MaxDiff, int preFilterCap, int uniquenessRatio, int speckleWindowSize, int speckleRange, int mode)
+ {
+
+ StereoSGBM retVal = new StereoSGBM(create_0(minDisparity, numDisparities, blockSize, P1, P2, disp12MaxDiff, preFilterCap, uniquenessRatio, speckleWindowSize, speckleRange, mode));
+
+ return retVal;
+ }
+
+ //javadoc: StereoSGBM::create()
+ public static StereoSGBM create()
+ {
+
+ StereoSGBM retVal = new StereoSGBM(create_1());
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getMode()
+ //
+
+ //javadoc: StereoSGBM::getMode()
+ public int getMode()
+ {
+
+ int retVal = getMode_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getP1()
+ //
+
+ //javadoc: StereoSGBM::getP1()
+ public int getP1()
+ {
+
+ int retVal = getP1_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getP2()
+ //
+
+ //javadoc: StereoSGBM::getP2()
+ public int getP2()
+ {
+
+ int retVal = getP2_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getPreFilterCap()
+ //
+
+ //javadoc: StereoSGBM::getPreFilterCap()
+ public int getPreFilterCap()
+ {
+
+ int retVal = getPreFilterCap_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getUniquenessRatio()
+ //
+
+ //javadoc: StereoSGBM::getUniquenessRatio()
+ public int getUniquenessRatio()
+ {
+
+ int retVal = getUniquenessRatio_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: void setMode(int mode)
+ //
+
+ //javadoc: StereoSGBM::setMode(mode)
+ public void setMode(int mode)
+ {
+
+ setMode_0(nativeObj, mode);
+
+ return;
+ }
+
+
+ //
+ // C++: void setP1(int P1)
+ //
+
+ //javadoc: StereoSGBM::setP1(P1)
+ public void setP1(int P1)
+ {
+
+ setP1_0(nativeObj, P1);
+
+ return;
+ }
+
+
+ //
+ // C++: void setP2(int P2)
+ //
+
+ //javadoc: StereoSGBM::setP2(P2)
+ public void setP2(int P2)
+ {
+
+ setP2_0(nativeObj, P2);
+
+ return;
+ }
+
+
+ //
+ // C++: void setPreFilterCap(int preFilterCap)
+ //
+
+ //javadoc: StereoSGBM::setPreFilterCap(preFilterCap)
+ public void setPreFilterCap(int preFilterCap)
+ {
+
+ setPreFilterCap_0(nativeObj, preFilterCap);
+
+ return;
+ }
+
+
+ //
+ // C++: void setUniquenessRatio(int uniquenessRatio)
+ //
+
+ //javadoc: StereoSGBM::setUniquenessRatio(uniquenessRatio)
+ public void setUniquenessRatio(int uniquenessRatio)
+ {
+
+ setUniquenessRatio_0(nativeObj, uniquenessRatio);
+
+ return;
+ }
+
+
+ @Override
+ protected void finalize() throws Throwable {
+ delete(nativeObj);
+ }
+
+
+
+ // C++: static Ptr_StereoSGBM create(int minDisparity = 0, int numDisparities = 16, int blockSize = 3, int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, int preFilterCap = 0, int uniquenessRatio = 0, int speckleWindowSize = 0, int speckleRange = 0, int mode = StereoSGBM::MODE_SGBM)
+ private static native long create_0(int minDisparity, int numDisparities, int blockSize, int P1, int P2, int disp12MaxDiff, int preFilterCap, int uniquenessRatio, int speckleWindowSize, int speckleRange, int mode);
+ private static native long create_1();
+
+ // C++: int getMode()
+ private static native int getMode_0(long nativeObj);
+
+ // C++: int getP1()
+ private static native int getP1_0(long nativeObj);
+
+ // C++: int getP2()
+ private static native int getP2_0(long nativeObj);
+
+ // C++: int getPreFilterCap()
+ private static native int getPreFilterCap_0(long nativeObj);
+
+ // C++: int getUniquenessRatio()
+ private static native int getUniquenessRatio_0(long nativeObj);
+
+ // C++: void setMode(int mode)
+ private static native void setMode_0(long nativeObj, int mode);
+
+ // C++: void setP1(int P1)
+ private static native void setP1_0(long nativeObj, int P1);
+
+ // C++: void setP2(int P2)
+ private static native void setP2_0(long nativeObj, int P2);
+
+ // C++: void setPreFilterCap(int preFilterCap)
+ private static native void setPreFilterCap_0(long nativeObj, int preFilterCap);
+
+ // C++: void setUniquenessRatio(int uniquenessRatio)
+ private static native void setUniquenessRatio_0(long nativeObj, int uniquenessRatio);
+
+ // native support for java finalize()
+ private static native void delete(long nativeObj);
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/Algorithm.java b/openCVLibrary330/src/main/java/org/opencv/core/Algorithm.java
new file mode 100644
index 00000000000..8437104c3b5
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/Algorithm.java
@@ -0,0 +1,79 @@
+
+//
+// This file is auto-generated. Please don't modify it!
+//
+package org.opencv.core;
+
+import java.lang.String;
+
+// C++: class Algorithm
+//javadoc: Algorithm
+public class Algorithm {
+
+ protected final long nativeObj;
+ protected Algorithm(long addr) { nativeObj = addr; }
+
+ public long getNativeObjAddr() { return nativeObj; }
+
+ //
+ // C++: String getDefaultName()
+ //
+
+ //javadoc: Algorithm::getDefaultName()
+ public String getDefaultName()
+ {
+
+ String retVal = getDefaultName_0(nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: void clear()
+ //
+
+ //javadoc: Algorithm::clear()
+ public void clear()
+ {
+
+ clear_0(nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void save(String filename)
+ //
+
+ //javadoc: Algorithm::save(filename)
+ public void save(String filename)
+ {
+
+ save_0(nativeObj, filename);
+
+ return;
+ }
+
+
+ @Override
+ protected void finalize() throws Throwable {
+ delete(nativeObj);
+ }
+
+
+
+ // C++: String getDefaultName()
+ private static native String getDefaultName_0(long nativeObj);
+
+ // C++: void clear()
+ private static native void clear_0(long nativeObj);
+
+ // C++: void save(String filename)
+ private static native void save_0(long nativeObj, String filename);
+
+ // native support for java finalize()
+ private static native void delete(long nativeObj);
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/Core.java b/openCVLibrary330/src/main/java/org/opencv/core/Core.java
new file mode 100644
index 00000000000..6f89b134ce1
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/Core.java
@@ -0,0 +1,2674 @@
+
+//
+// This file is auto-generated. Please don't modify it!
+//
+package org.opencv.core;
+
+import java.lang.String;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfInt;
+import org.opencv.core.Scalar;
+import org.opencv.core.TermCriteria;
+import org.opencv.utils.Converters;
+
+public class Core {
+
+ // these constants are wrapped inside functions to prevent inlining
+ private static String getVersion() { return "3.3.0"; }
+ private static String getNativeLibraryName() { return "opencv_java330"; }
+ private static int getVersionMajor() { return 3; }
+ private static int getVersionMinor() { return 3; }
+ private static int getVersionRevision() { return 0; }
+ private static String getVersionStatus() { return ""; }
+
+ public static final String VERSION = getVersion();
+ public static final String NATIVE_LIBRARY_NAME = getNativeLibraryName();
+ public static final int VERSION_MAJOR = getVersionMajor();
+ public static final int VERSION_MINOR = getVersionMinor();
+ public static final int VERSION_REVISION = getVersionRevision();
+ public static final String VERSION_STATUS = getVersionStatus();
+
+ private static final int
+ CV_8U = 0,
+ CV_8S = 1,
+ CV_16U = 2,
+ CV_16S = 3,
+ CV_32S = 4,
+ CV_32F = 5,
+ CV_64F = 6,
+ CV_USRTYPE1 = 7;
+
+
+ public static final int
+ SVD_MODIFY_A = 1,
+ SVD_NO_UV = 2,
+ SVD_FULL_UV = 4,
+ FILLED = -1,
+ REDUCE_SUM = 0,
+ REDUCE_AVG = 1,
+ REDUCE_MAX = 2,
+ REDUCE_MIN = 3,
+ StsOk = 0,
+ StsBackTrace = -1,
+ StsError = -2,
+ StsInternal = -3,
+ StsNoMem = -4,
+ StsBadArg = -5,
+ StsBadFunc = -6,
+ StsNoConv = -7,
+ StsAutoTrace = -8,
+ HeaderIsNull = -9,
+ BadImageSize = -10,
+ BadOffset = -11,
+ BadDataPtr = -12,
+ BadStep = -13,
+ BadModelOrChSeq = -14,
+ BadNumChannels = -15,
+ BadNumChannel1U = -16,
+ BadDepth = -17,
+ BadAlphaChannel = -18,
+ BadOrder = -19,
+ BadOrigin = -20,
+ BadAlign = -21,
+ BadCallBack = -22,
+ BadTileSize = -23,
+ BadCOI = -24,
+ BadROISize = -25,
+ MaskIsTiled = -26,
+ StsNullPtr = -27,
+ StsVecLengthErr = -28,
+ StsFilterStructContentErr = -29,
+ StsKernelStructContentErr = -30,
+ StsFilterOffsetErr = -31,
+ StsBadSize = -201,
+ StsDivByZero = -202,
+ StsInplaceNotSupported = -203,
+ StsObjectNotFound = -204,
+ StsUnmatchedFormats = -205,
+ StsBadFlag = -206,
+ StsBadPoint = -207,
+ StsBadMask = -208,
+ StsUnmatchedSizes = -209,
+ StsUnsupportedFormat = -210,
+ StsOutOfRange = -211,
+ StsParseError = -212,
+ StsNotImplemented = -213,
+ StsBadMemBlock = -214,
+ StsAssert = -215,
+ GpuNotSupported = -216,
+ GpuApiCallError = -217,
+ OpenGlNotSupported = -218,
+ OpenGlApiCallError = -219,
+ OpenCLApiCallError = -220,
+ OpenCLDoubleNotSupported = -221,
+ OpenCLInitError = -222,
+ OpenCLNoAMDBlasFft = -223,
+ DECOMP_LU = 0,
+ DECOMP_SVD = 1,
+ DECOMP_EIG = 2,
+ DECOMP_CHOLESKY = 3,
+ DECOMP_QR = 4,
+ DECOMP_NORMAL = 16,
+ NORM_INF = 1,
+ NORM_L1 = 2,
+ NORM_L2 = 4,
+ NORM_L2SQR = 5,
+ NORM_HAMMING = 6,
+ NORM_HAMMING2 = 7,
+ NORM_TYPE_MASK = 7,
+ NORM_RELATIVE = 8,
+ NORM_MINMAX = 32,
+ CMP_EQ = 0,
+ CMP_GT = 1,
+ CMP_GE = 2,
+ CMP_LT = 3,
+ CMP_LE = 4,
+ CMP_NE = 5,
+ GEMM_1_T = 1,
+ GEMM_2_T = 2,
+ GEMM_3_T = 4,
+ DFT_INVERSE = 1,
+ DFT_SCALE = 2,
+ DFT_ROWS = 4,
+ DFT_COMPLEX_OUTPUT = 16,
+ DFT_REAL_OUTPUT = 32,
+ DFT_COMPLEX_INPUT = 64,
+ DCT_INVERSE = DFT_INVERSE,
+ DCT_ROWS = DFT_ROWS,
+ BORDER_CONSTANT = 0,
+ BORDER_REPLICATE = 1,
+ BORDER_REFLECT = 2,
+ BORDER_WRAP = 3,
+ BORDER_REFLECT_101 = 4,
+ BORDER_TRANSPARENT = 5,
+ BORDER_REFLECT101 = BORDER_REFLECT_101,
+ BORDER_DEFAULT = BORDER_REFLECT_101,
+ BORDER_ISOLATED = 16,
+ SORT_EVERY_ROW = 0,
+ SORT_EVERY_COLUMN = 1,
+ SORT_ASCENDING = 0,
+ SORT_DESCENDING = 16,
+ COVAR_SCRAMBLED = 0,
+ COVAR_NORMAL = 1,
+ COVAR_USE_AVG = 2,
+ COVAR_SCALE = 4,
+ COVAR_ROWS = 8,
+ COVAR_COLS = 16,
+ KMEANS_RANDOM_CENTERS = 0,
+ KMEANS_PP_CENTERS = 2,
+ KMEANS_USE_INITIAL_LABELS = 1,
+ LINE_4 = 4,
+ LINE_8 = 8,
+ LINE_AA = 16,
+ FONT_HERSHEY_SIMPLEX = 0,
+ FONT_HERSHEY_PLAIN = 1,
+ FONT_HERSHEY_DUPLEX = 2,
+ FONT_HERSHEY_COMPLEX = 3,
+ FONT_HERSHEY_TRIPLEX = 4,
+ FONT_HERSHEY_COMPLEX_SMALL = 5,
+ FONT_HERSHEY_SCRIPT_SIMPLEX = 6,
+ FONT_HERSHEY_SCRIPT_COMPLEX = 7,
+ FONT_ITALIC = 16,
+ ROTATE_90_CLOCKWISE = 0,
+ ROTATE_180 = 1,
+ ROTATE_90_COUNTERCLOCKWISE = 2,
+ TYPE_GENERAL = 0,
+ TYPE_MARKER = 0+1,
+ TYPE_WRAPPER = 0+2,
+ TYPE_FUN = 0+3,
+ IMPL_PLAIN = 0,
+ IMPL_IPP = 0+1,
+ IMPL_OPENCL = 0+2,
+ FLAGS_NONE = 0,
+ FLAGS_MAPPING = 0x01,
+ FLAGS_EXPAND_SAME_NAMES = 0x02;
+
+
+ //
+ // C++: Scalar mean(Mat src, Mat mask = Mat())
+ //
+
+ //javadoc: mean(src, mask)
+ public static Scalar mean(Mat src, Mat mask)
+ {
+
+ Scalar retVal = new Scalar(mean_0(src.nativeObj, mask.nativeObj));
+
+ return retVal;
+ }
+
+ //javadoc: mean(src)
+ public static Scalar mean(Mat src)
+ {
+
+ Scalar retVal = new Scalar(mean_1(src.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Scalar sum(Mat src)
+ //
+
+ //javadoc: sum(src)
+ public static Scalar sumElems(Mat src)
+ {
+
+ Scalar retVal = new Scalar(sumElems_0(src.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: Scalar trace(Mat mtx)
+ //
+
+ //javadoc: trace(mtx)
+ public static Scalar trace(Mat mtx)
+ {
+
+ Scalar retVal = new Scalar(trace_0(mtx.nativeObj));
+
+ return retVal;
+ }
+
+
+ //
+ // C++: String getBuildInformation()
+ //
+
+ //javadoc: getBuildInformation()
+ public static String getBuildInformation()
+ {
+
+ String retVal = getBuildInformation_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool checkRange(Mat a, bool quiet = true, _hidden_ * pos = 0, double minVal = -DBL_MAX, double maxVal = DBL_MAX)
+ //
+
+ //javadoc: checkRange(a, quiet, minVal, maxVal)
+ public static boolean checkRange(Mat a, boolean quiet, double minVal, double maxVal)
+ {
+
+ boolean retVal = checkRange_0(a.nativeObj, quiet, minVal, maxVal);
+
+ return retVal;
+ }
+
+ //javadoc: checkRange(a)
+ public static boolean checkRange(Mat a)
+ {
+
+ boolean retVal = checkRange_1(a.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool eigen(Mat src, Mat& eigenvalues, Mat& eigenvectors = Mat())
+ //
+
+ //javadoc: eigen(src, eigenvalues, eigenvectors)
+ public static boolean eigen(Mat src, Mat eigenvalues, Mat eigenvectors)
+ {
+
+ boolean retVal = eigen_0(src.nativeObj, eigenvalues.nativeObj, eigenvectors.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: eigen(src, eigenvalues)
+ public static boolean eigen(Mat src, Mat eigenvalues)
+ {
+
+ boolean retVal = eigen_1(src.nativeObj, eigenvalues.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool solve(Mat src1, Mat src2, Mat& dst, int flags = DECOMP_LU)
+ //
+
+ //javadoc: solve(src1, src2, dst, flags)
+ public static boolean solve(Mat src1, Mat src2, Mat dst, int flags)
+ {
+
+ boolean retVal = solve_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: solve(src1, src2, dst)
+ public static boolean solve(Mat src1, Mat src2, Mat dst)
+ {
+
+ boolean retVal = solve_1(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: bool useIPP()
+ //
+
+ //javadoc: useIPP()
+ public static boolean useIPP()
+ {
+
+ boolean retVal = useIPP_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double Mahalanobis(Mat v1, Mat v2, Mat icovar)
+ //
+
+ //javadoc: Mahalanobis(v1, v2, icovar)
+ public static double Mahalanobis(Mat v1, Mat v2, Mat icovar)
+ {
+
+ double retVal = Mahalanobis_0(v1.nativeObj, v2.nativeObj, icovar.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double PSNR(Mat src1, Mat src2)
+ //
+
+ //javadoc: PSNR(src1, src2)
+ public static double PSNR(Mat src1, Mat src2)
+ {
+
+ double retVal = PSNR_0(src1.nativeObj, src2.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double determinant(Mat mtx)
+ //
+
+ //javadoc: determinant(mtx)
+ public static double determinant(Mat mtx)
+ {
+
+ double retVal = determinant_0(mtx.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double getTickFrequency()
+ //
+
+ //javadoc: getTickFrequency()
+ public static double getTickFrequency()
+ {
+
+ double retVal = getTickFrequency_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double invert(Mat src, Mat& dst, int flags = DECOMP_LU)
+ //
+
+ //javadoc: invert(src, dst, flags)
+ public static double invert(Mat src, Mat dst, int flags)
+ {
+
+ double retVal = invert_0(src.nativeObj, dst.nativeObj, flags);
+
+ return retVal;
+ }
+
+ //javadoc: invert(src, dst)
+ public static double invert(Mat src, Mat dst)
+ {
+
+ double retVal = invert_1(src.nativeObj, dst.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double kmeans(Mat data, int K, Mat& bestLabels, TermCriteria criteria, int attempts, int flags, Mat& centers = Mat())
+ //
+
+ //javadoc: kmeans(data, K, bestLabels, criteria, attempts, flags, centers)
+ public static double kmeans(Mat data, int K, Mat bestLabels, TermCriteria criteria, int attempts, int flags, Mat centers)
+ {
+
+ double retVal = kmeans_0(data.nativeObj, K, bestLabels.nativeObj, criteria.type, criteria.maxCount, criteria.epsilon, attempts, flags, centers.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: kmeans(data, K, bestLabels, criteria, attempts, flags)
+ public static double kmeans(Mat data, int K, Mat bestLabels, TermCriteria criteria, int attempts, int flags)
+ {
+
+ double retVal = kmeans_1(data.nativeObj, K, bestLabels.nativeObj, criteria.type, criteria.maxCount, criteria.epsilon, attempts, flags);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double norm(Mat src1, Mat src2, int normType = NORM_L2, Mat mask = Mat())
+ //
+
+ //javadoc: norm(src1, src2, normType, mask)
+ public static double norm(Mat src1, Mat src2, int normType, Mat mask)
+ {
+
+ double retVal = norm_0(src1.nativeObj, src2.nativeObj, normType, mask.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: norm(src1, src2, normType)
+ public static double norm(Mat src1, Mat src2, int normType)
+ {
+
+ double retVal = norm_1(src1.nativeObj, src2.nativeObj, normType);
+
+ return retVal;
+ }
+
+ //javadoc: norm(src1, src2)
+ public static double norm(Mat src1, Mat src2)
+ {
+
+ double retVal = norm_2(src1.nativeObj, src2.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double norm(Mat src1, int normType = NORM_L2, Mat mask = Mat())
+ //
+
+ //javadoc: norm(src1, normType, mask)
+ public static double norm(Mat src1, int normType, Mat mask)
+ {
+
+ double retVal = norm_3(src1.nativeObj, normType, mask.nativeObj);
+
+ return retVal;
+ }
+
+ //javadoc: norm(src1, normType)
+ public static double norm(Mat src1, int normType)
+ {
+
+ double retVal = norm_4(src1.nativeObj, normType);
+
+ return retVal;
+ }
+
+ //javadoc: norm(src1)
+ public static double norm(Mat src1)
+ {
+
+ double retVal = norm_5(src1.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: double solvePoly(Mat coeffs, Mat& roots, int maxIters = 300)
+ //
+
+ //javadoc: solvePoly(coeffs, roots, maxIters)
+ public static double solvePoly(Mat coeffs, Mat roots, int maxIters)
+ {
+
+ double retVal = solvePoly_0(coeffs.nativeObj, roots.nativeObj, maxIters);
+
+ return retVal;
+ }
+
+ //javadoc: solvePoly(coeffs, roots)
+ public static double solvePoly(Mat coeffs, Mat roots)
+ {
+
+ double retVal = solvePoly_1(coeffs.nativeObj, roots.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: float cubeRoot(float val)
+ //
+
+ //javadoc: cubeRoot(val)
+ public static float cubeRoot(float val)
+ {
+
+ float retVal = cubeRoot_0(val);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: float fastAtan2(float y, float x)
+ //
+
+ //javadoc: fastAtan2(y, x)
+ public static float fastAtan2(float y, float x)
+ {
+
+ float retVal = fastAtan2_0(y, x);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int borderInterpolate(int p, int len, int borderType)
+ //
+
+ //javadoc: borderInterpolate(p, len, borderType)
+ public static int borderInterpolate(int p, int len, int borderType)
+ {
+
+ int retVal = borderInterpolate_0(p, len, borderType);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int countNonZero(Mat src)
+ //
+
+ //javadoc: countNonZero(src)
+ public static int countNonZero(Mat src)
+ {
+
+ int retVal = countNonZero_0(src.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getNumThreads()
+ //
+
+ //javadoc: getNumThreads()
+ public static int getNumThreads()
+ {
+
+ int retVal = getNumThreads_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getNumberOfCPUs()
+ //
+
+ //javadoc: getNumberOfCPUs()
+ public static int getNumberOfCPUs()
+ {
+
+ int retVal = getNumberOfCPUs_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getOptimalDFTSize(int vecsize)
+ //
+
+ //javadoc: getOptimalDFTSize(vecsize)
+ public static int getOptimalDFTSize(int vecsize)
+ {
+
+ int retVal = getOptimalDFTSize_0(vecsize);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int getThreadNum()
+ //
+
+ //javadoc: getThreadNum()
+ public static int getThreadNum()
+ {
+
+ int retVal = getThreadNum_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int solveCubic(Mat coeffs, Mat& roots)
+ //
+
+ //javadoc: solveCubic(coeffs, roots)
+ public static int solveCubic(Mat coeffs, Mat roots)
+ {
+
+ int retVal = solveCubic_0(coeffs.nativeObj, roots.nativeObj);
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int64 getCPUTickCount()
+ //
+
+ //javadoc: getCPUTickCount()
+ public static long getCPUTickCount()
+ {
+
+ long retVal = getCPUTickCount_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: int64 getTickCount()
+ //
+
+ //javadoc: getTickCount()
+ public static long getTickCount()
+ {
+
+ long retVal = getTickCount_0();
+
+ return retVal;
+ }
+
+
+ //
+ // C++: void LUT(Mat src, Mat lut, Mat& dst)
+ //
+
+ //javadoc: LUT(src, lut, dst)
+ public static void LUT(Mat src, Mat lut, Mat dst)
+ {
+
+ LUT_0(src.nativeObj, lut.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void PCABackProject(Mat data, Mat mean, Mat eigenvectors, Mat& result)
+ //
+
+ //javadoc: PCABackProject(data, mean, eigenvectors, result)
+ public static void PCABackProject(Mat data, Mat mean, Mat eigenvectors, Mat result)
+ {
+
+ PCABackProject_0(data.nativeObj, mean.nativeObj, eigenvectors.nativeObj, result.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void PCACompute(Mat data, Mat& mean, Mat& eigenvectors, double retainedVariance)
+ //
+
+ //javadoc: PCACompute(data, mean, eigenvectors, retainedVariance)
+ public static void PCACompute(Mat data, Mat mean, Mat eigenvectors, double retainedVariance)
+ {
+
+ PCACompute_0(data.nativeObj, mean.nativeObj, eigenvectors.nativeObj, retainedVariance);
+
+ return;
+ }
+
+
+ //
+ // C++: void PCACompute(Mat data, Mat& mean, Mat& eigenvectors, int maxComponents = 0)
+ //
+
+ //javadoc: PCACompute(data, mean, eigenvectors, maxComponents)
+ public static void PCACompute(Mat data, Mat mean, Mat eigenvectors, int maxComponents)
+ {
+
+ PCACompute_1(data.nativeObj, mean.nativeObj, eigenvectors.nativeObj, maxComponents);
+
+ return;
+ }
+
+ //javadoc: PCACompute(data, mean, eigenvectors)
+ public static void PCACompute(Mat data, Mat mean, Mat eigenvectors)
+ {
+
+ PCACompute_2(data.nativeObj, mean.nativeObj, eigenvectors.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void PCAProject(Mat data, Mat mean, Mat eigenvectors, Mat& result)
+ //
+
+ //javadoc: PCAProject(data, mean, eigenvectors, result)
+ public static void PCAProject(Mat data, Mat mean, Mat eigenvectors, Mat result)
+ {
+
+ PCAProject_0(data.nativeObj, mean.nativeObj, eigenvectors.nativeObj, result.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void SVBackSubst(Mat w, Mat u, Mat vt, Mat rhs, Mat& dst)
+ //
+
+ //javadoc: SVBackSubst(w, u, vt, rhs, dst)
+ public static void SVBackSubst(Mat w, Mat u, Mat vt, Mat rhs, Mat dst)
+ {
+
+ SVBackSubst_0(w.nativeObj, u.nativeObj, vt.nativeObj, rhs.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void SVDecomp(Mat src, Mat& w, Mat& u, Mat& vt, int flags = 0)
+ //
+
+ //javadoc: SVDecomp(src, w, u, vt, flags)
+ public static void SVDecomp(Mat src, Mat w, Mat u, Mat vt, int flags)
+ {
+
+ SVDecomp_0(src.nativeObj, w.nativeObj, u.nativeObj, vt.nativeObj, flags);
+
+ return;
+ }
+
+ //javadoc: SVDecomp(src, w, u, vt)
+ public static void SVDecomp(Mat src, Mat w, Mat u, Mat vt)
+ {
+
+ SVDecomp_1(src.nativeObj, w.nativeObj, u.nativeObj, vt.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void absdiff(Mat src1, Mat src2, Mat& dst)
+ //
+
+ //javadoc: absdiff(src1, src2, dst)
+ public static void absdiff(Mat src1, Mat src2, Mat dst)
+ {
+
+ absdiff_0(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void absdiff(Mat src1, Scalar src2, Mat& dst)
+ //
+
+ //javadoc: absdiff(src1, src2, dst)
+ public static void absdiff(Mat src1, Scalar src2, Mat dst)
+ {
+
+ absdiff_1(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void add(Mat src1, Mat src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ //
+
+ //javadoc: add(src1, src2, dst, mask, dtype)
+ public static void add(Mat src1, Mat src2, Mat dst, Mat mask, int dtype)
+ {
+
+ add_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj, dtype);
+
+ return;
+ }
+
+ //javadoc: add(src1, src2, dst, mask)
+ public static void add(Mat src1, Mat src2, Mat dst, Mat mask)
+ {
+
+ add_1(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: add(src1, src2, dst)
+ public static void add(Mat src1, Mat src2, Mat dst)
+ {
+
+ add_2(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void add(Mat src1, Scalar src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ //
+
+ //javadoc: add(src1, src2, dst, mask, dtype)
+ public static void add(Mat src1, Scalar src2, Mat dst, Mat mask, int dtype)
+ {
+
+ add_3(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, mask.nativeObj, dtype);
+
+ return;
+ }
+
+ //javadoc: add(src1, src2, dst, mask)
+ public static void add(Mat src1, Scalar src2, Mat dst, Mat mask)
+ {
+
+ add_4(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: add(src1, src2, dst)
+ public static void add(Mat src1, Scalar src2, Mat dst)
+ {
+
+ add_5(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void addWeighted(Mat src1, double alpha, Mat src2, double beta, double gamma, Mat& dst, int dtype = -1)
+ //
+
+ //javadoc: addWeighted(src1, alpha, src2, beta, gamma, dst, dtype)
+ public static void addWeighted(Mat src1, double alpha, Mat src2, double beta, double gamma, Mat dst, int dtype)
+ {
+
+ addWeighted_0(src1.nativeObj, alpha, src2.nativeObj, beta, gamma, dst.nativeObj, dtype);
+
+ return;
+ }
+
+ //javadoc: addWeighted(src1, alpha, src2, beta, gamma, dst)
+ public static void addWeighted(Mat src1, double alpha, Mat src2, double beta, double gamma, Mat dst)
+ {
+
+ addWeighted_1(src1.nativeObj, alpha, src2.nativeObj, beta, gamma, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void batchDistance(Mat src1, Mat src2, Mat& dist, int dtype, Mat& nidx, int normType = NORM_L2, int K = 0, Mat mask = Mat(), int update = 0, bool crosscheck = false)
+ //
+
+ //javadoc: batchDistance(src1, src2, dist, dtype, nidx, normType, K, mask, update, crosscheck)
+ public static void batchDistance(Mat src1, Mat src2, Mat dist, int dtype, Mat nidx, int normType, int K, Mat mask, int update, boolean crosscheck)
+ {
+
+ batchDistance_0(src1.nativeObj, src2.nativeObj, dist.nativeObj, dtype, nidx.nativeObj, normType, K, mask.nativeObj, update, crosscheck);
+
+ return;
+ }
+
+ //javadoc: batchDistance(src1, src2, dist, dtype, nidx, normType, K)
+ public static void batchDistance(Mat src1, Mat src2, Mat dist, int dtype, Mat nidx, int normType, int K)
+ {
+
+ batchDistance_1(src1.nativeObj, src2.nativeObj, dist.nativeObj, dtype, nidx.nativeObj, normType, K);
+
+ return;
+ }
+
+ //javadoc: batchDistance(src1, src2, dist, dtype, nidx)
+ public static void batchDistance(Mat src1, Mat src2, Mat dist, int dtype, Mat nidx)
+ {
+
+ batchDistance_2(src1.nativeObj, src2.nativeObj, dist.nativeObj, dtype, nidx.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void bitwise_and(Mat src1, Mat src2, Mat& dst, Mat mask = Mat())
+ //
+
+ //javadoc: bitwise_and(src1, src2, dst, mask)
+ public static void bitwise_and(Mat src1, Mat src2, Mat dst, Mat mask)
+ {
+
+ bitwise_and_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: bitwise_and(src1, src2, dst)
+ public static void bitwise_and(Mat src1, Mat src2, Mat dst)
+ {
+
+ bitwise_and_1(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void bitwise_not(Mat src, Mat& dst, Mat mask = Mat())
+ //
+
+ //javadoc: bitwise_not(src, dst, mask)
+ public static void bitwise_not(Mat src, Mat dst, Mat mask)
+ {
+
+ bitwise_not_0(src.nativeObj, dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: bitwise_not(src, dst)
+ public static void bitwise_not(Mat src, Mat dst)
+ {
+
+ bitwise_not_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void bitwise_or(Mat src1, Mat src2, Mat& dst, Mat mask = Mat())
+ //
+
+ //javadoc: bitwise_or(src1, src2, dst, mask)
+ public static void bitwise_or(Mat src1, Mat src2, Mat dst, Mat mask)
+ {
+
+ bitwise_or_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: bitwise_or(src1, src2, dst)
+ public static void bitwise_or(Mat src1, Mat src2, Mat dst)
+ {
+
+ bitwise_or_1(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void bitwise_xor(Mat src1, Mat src2, Mat& dst, Mat mask = Mat())
+ //
+
+ //javadoc: bitwise_xor(src1, src2, dst, mask)
+ public static void bitwise_xor(Mat src1, Mat src2, Mat dst, Mat mask)
+ {
+
+ bitwise_xor_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: bitwise_xor(src1, src2, dst)
+ public static void bitwise_xor(Mat src1, Mat src2, Mat dst)
+ {
+
+ bitwise_xor_1(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void calcCovarMatrix(Mat samples, Mat& covar, Mat& mean, int flags, int ctype = CV_64F)
+ //
+
+ //javadoc: calcCovarMatrix(samples, covar, mean, flags, ctype)
+ public static void calcCovarMatrix(Mat samples, Mat covar, Mat mean, int flags, int ctype)
+ {
+
+ calcCovarMatrix_0(samples.nativeObj, covar.nativeObj, mean.nativeObj, flags, ctype);
+
+ return;
+ }
+
+ //javadoc: calcCovarMatrix(samples, covar, mean, flags)
+ public static void calcCovarMatrix(Mat samples, Mat covar, Mat mean, int flags)
+ {
+
+ calcCovarMatrix_1(samples.nativeObj, covar.nativeObj, mean.nativeObj, flags);
+
+ return;
+ }
+
+
+ //
+ // C++: void cartToPolar(Mat x, Mat y, Mat& magnitude, Mat& angle, bool angleInDegrees = false)
+ //
+
+ //javadoc: cartToPolar(x, y, magnitude, angle, angleInDegrees)
+ public static void cartToPolar(Mat x, Mat y, Mat magnitude, Mat angle, boolean angleInDegrees)
+ {
+
+ cartToPolar_0(x.nativeObj, y.nativeObj, magnitude.nativeObj, angle.nativeObj, angleInDegrees);
+
+ return;
+ }
+
+ //javadoc: cartToPolar(x, y, magnitude, angle)
+ public static void cartToPolar(Mat x, Mat y, Mat magnitude, Mat angle)
+ {
+
+ cartToPolar_1(x.nativeObj, y.nativeObj, magnitude.nativeObj, angle.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void compare(Mat src1, Mat src2, Mat& dst, int cmpop)
+ //
+
+ //javadoc: compare(src1, src2, dst, cmpop)
+ public static void compare(Mat src1, Mat src2, Mat dst, int cmpop)
+ {
+
+ compare_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, cmpop);
+
+ return;
+ }
+
+
+ //
+ // C++: void compare(Mat src1, Scalar src2, Mat& dst, int cmpop)
+ //
+
+ //javadoc: compare(src1, src2, dst, cmpop)
+ public static void compare(Mat src1, Scalar src2, Mat dst, int cmpop)
+ {
+
+ compare_1(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, cmpop);
+
+ return;
+ }
+
+
+ //
+ // C++: void completeSymm(Mat& mtx, bool lowerToUpper = false)
+ //
+
+ //javadoc: completeSymm(mtx, lowerToUpper)
+ public static void completeSymm(Mat mtx, boolean lowerToUpper)
+ {
+
+ completeSymm_0(mtx.nativeObj, lowerToUpper);
+
+ return;
+ }
+
+ //javadoc: completeSymm(mtx)
+ public static void completeSymm(Mat mtx)
+ {
+
+ completeSymm_1(mtx.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void convertFp16(Mat src, Mat& dst)
+ //
+
+ //javadoc: convertFp16(src, dst)
+ public static void convertFp16(Mat src, Mat dst)
+ {
+
+ convertFp16_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void convertScaleAbs(Mat src, Mat& dst, double alpha = 1, double beta = 0)
+ //
+
+ //javadoc: convertScaleAbs(src, dst, alpha, beta)
+ public static void convertScaleAbs(Mat src, Mat dst, double alpha, double beta)
+ {
+
+ convertScaleAbs_0(src.nativeObj, dst.nativeObj, alpha, beta);
+
+ return;
+ }
+
+ //javadoc: convertScaleAbs(src, dst)
+ public static void convertScaleAbs(Mat src, Mat dst)
+ {
+
+ convertScaleAbs_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void copyMakeBorder(Mat src, Mat& dst, int top, int bottom, int left, int right, int borderType, Scalar value = Scalar())
+ //
+
+ //javadoc: copyMakeBorder(src, dst, top, bottom, left, right, borderType, value)
+ public static void copyMakeBorder(Mat src, Mat dst, int top, int bottom, int left, int right, int borderType, Scalar value)
+ {
+
+ copyMakeBorder_0(src.nativeObj, dst.nativeObj, top, bottom, left, right, borderType, value.val[0], value.val[1], value.val[2], value.val[3]);
+
+ return;
+ }
+
+ //javadoc: copyMakeBorder(src, dst, top, bottom, left, right, borderType)
+ public static void copyMakeBorder(Mat src, Mat dst, int top, int bottom, int left, int right, int borderType)
+ {
+
+ copyMakeBorder_1(src.nativeObj, dst.nativeObj, top, bottom, left, right, borderType);
+
+ return;
+ }
+
+
+ //
+ // C++: void dct(Mat src, Mat& dst, int flags = 0)
+ //
+
+ //javadoc: dct(src, dst, flags)
+ public static void dct(Mat src, Mat dst, int flags)
+ {
+
+ dct_0(src.nativeObj, dst.nativeObj, flags);
+
+ return;
+ }
+
+ //javadoc: dct(src, dst)
+ public static void dct(Mat src, Mat dst)
+ {
+
+ dct_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void dft(Mat src, Mat& dst, int flags = 0, int nonzeroRows = 0)
+ //
+
+ //javadoc: dft(src, dst, flags, nonzeroRows)
+ public static void dft(Mat src, Mat dst, int flags, int nonzeroRows)
+ {
+
+ dft_0(src.nativeObj, dst.nativeObj, flags, nonzeroRows);
+
+ return;
+ }
+
+ //javadoc: dft(src, dst)
+ public static void dft(Mat src, Mat dst)
+ {
+
+ dft_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void divide(Mat src1, Mat src2, Mat& dst, double scale = 1, int dtype = -1)
+ //
+
+ //javadoc: divide(src1, src2, dst, scale, dtype)
+ public static void divide(Mat src1, Mat src2, Mat dst, double scale, int dtype)
+ {
+
+ divide_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, scale, dtype);
+
+ return;
+ }
+
+ //javadoc: divide(src1, src2, dst, scale)
+ public static void divide(Mat src1, Mat src2, Mat dst, double scale)
+ {
+
+ divide_1(src1.nativeObj, src2.nativeObj, dst.nativeObj, scale);
+
+ return;
+ }
+
+ //javadoc: divide(src1, src2, dst)
+ public static void divide(Mat src1, Mat src2, Mat dst)
+ {
+
+ divide_2(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void divide(Mat src1, Scalar src2, Mat& dst, double scale = 1, int dtype = -1)
+ //
+
+ //javadoc: divide(src1, src2, dst, scale, dtype)
+ public static void divide(Mat src1, Scalar src2, Mat dst, double scale, int dtype)
+ {
+
+ divide_3(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, scale, dtype);
+
+ return;
+ }
+
+ //javadoc: divide(src1, src2, dst, scale)
+ public static void divide(Mat src1, Scalar src2, Mat dst, double scale)
+ {
+
+ divide_4(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, scale);
+
+ return;
+ }
+
+ //javadoc: divide(src1, src2, dst)
+ public static void divide(Mat src1, Scalar src2, Mat dst)
+ {
+
+ divide_5(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void divide(double scale, Mat src2, Mat& dst, int dtype = -1)
+ //
+
+ //javadoc: divide(scale, src2, dst, dtype)
+ public static void divide(double scale, Mat src2, Mat dst, int dtype)
+ {
+
+ divide_6(scale, src2.nativeObj, dst.nativeObj, dtype);
+
+ return;
+ }
+
+ //javadoc: divide(scale, src2, dst)
+ public static void divide(double scale, Mat src2, Mat dst)
+ {
+
+ divide_7(scale, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void exp(Mat src, Mat& dst)
+ //
+
+ //javadoc: exp(src, dst)
+ public static void exp(Mat src, Mat dst)
+ {
+
+ exp_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void extractChannel(Mat src, Mat& dst, int coi)
+ //
+
+ //javadoc: extractChannel(src, dst, coi)
+ public static void extractChannel(Mat src, Mat dst, int coi)
+ {
+
+ extractChannel_0(src.nativeObj, dst.nativeObj, coi);
+
+ return;
+ }
+
+
+ //
+ // C++: void findNonZero(Mat src, Mat& idx)
+ //
+
+ //javadoc: findNonZero(src, idx)
+ public static void findNonZero(Mat src, Mat idx)
+ {
+
+ findNonZero_0(src.nativeObj, idx.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void flip(Mat src, Mat& dst, int flipCode)
+ //
+
+ //javadoc: flip(src, dst, flipCode)
+ public static void flip(Mat src, Mat dst, int flipCode)
+ {
+
+ flip_0(src.nativeObj, dst.nativeObj, flipCode);
+
+ return;
+ }
+
+
+ //
+ // C++: void gemm(Mat src1, Mat src2, double alpha, Mat src3, double beta, Mat& dst, int flags = 0)
+ //
+
+ //javadoc: gemm(src1, src2, alpha, src3, beta, dst, flags)
+ public static void gemm(Mat src1, Mat src2, double alpha, Mat src3, double beta, Mat dst, int flags)
+ {
+
+ gemm_0(src1.nativeObj, src2.nativeObj, alpha, src3.nativeObj, beta, dst.nativeObj, flags);
+
+ return;
+ }
+
+ //javadoc: gemm(src1, src2, alpha, src3, beta, dst)
+ public static void gemm(Mat src1, Mat src2, double alpha, Mat src3, double beta, Mat dst)
+ {
+
+ gemm_1(src1.nativeObj, src2.nativeObj, alpha, src3.nativeObj, beta, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void hconcat(vector_Mat src, Mat& dst)
+ //
+
+ //javadoc: hconcat(src, dst)
+ public static void hconcat(List src, Mat dst)
+ {
+ Mat src_mat = Converters.vector_Mat_to_Mat(src);
+ hconcat_0(src_mat.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void idct(Mat src, Mat& dst, int flags = 0)
+ //
+
+ //javadoc: idct(src, dst, flags)
+ public static void idct(Mat src, Mat dst, int flags)
+ {
+
+ idct_0(src.nativeObj, dst.nativeObj, flags);
+
+ return;
+ }
+
+ //javadoc: idct(src, dst)
+ public static void idct(Mat src, Mat dst)
+ {
+
+ idct_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void idft(Mat src, Mat& dst, int flags = 0, int nonzeroRows = 0)
+ //
+
+ //javadoc: idft(src, dst, flags, nonzeroRows)
+ public static void idft(Mat src, Mat dst, int flags, int nonzeroRows)
+ {
+
+ idft_0(src.nativeObj, dst.nativeObj, flags, nonzeroRows);
+
+ return;
+ }
+
+ //javadoc: idft(src, dst)
+ public static void idft(Mat src, Mat dst)
+ {
+
+ idft_1(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void inRange(Mat src, Scalar lowerb, Scalar upperb, Mat& dst)
+ //
+
+ //javadoc: inRange(src, lowerb, upperb, dst)
+ public static void inRange(Mat src, Scalar lowerb, Scalar upperb, Mat dst)
+ {
+
+ inRange_0(src.nativeObj, lowerb.val[0], lowerb.val[1], lowerb.val[2], lowerb.val[3], upperb.val[0], upperb.val[1], upperb.val[2], upperb.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void insertChannel(Mat src, Mat& dst, int coi)
+ //
+
+ //javadoc: insertChannel(src, dst, coi)
+ public static void insertChannel(Mat src, Mat dst, int coi)
+ {
+
+ insertChannel_0(src.nativeObj, dst.nativeObj, coi);
+
+ return;
+ }
+
+
+ //
+ // C++: void log(Mat src, Mat& dst)
+ //
+
+ //javadoc: log(src, dst)
+ public static void log(Mat src, Mat dst)
+ {
+
+ log_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void magnitude(Mat x, Mat y, Mat& magnitude)
+ //
+
+ //javadoc: magnitude(x, y, magnitude)
+ public static void magnitude(Mat x, Mat y, Mat magnitude)
+ {
+
+ magnitude_0(x.nativeObj, y.nativeObj, magnitude.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void max(Mat src1, Mat src2, Mat& dst)
+ //
+
+ //javadoc: max(src1, src2, dst)
+ public static void max(Mat src1, Mat src2, Mat dst)
+ {
+
+ max_0(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void max(Mat src1, Scalar src2, Mat& dst)
+ //
+
+ //javadoc: max(src1, src2, dst)
+ public static void max(Mat src1, Scalar src2, Mat dst)
+ {
+
+ max_1(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void meanStdDev(Mat src, vector_double& mean, vector_double& stddev, Mat mask = Mat())
+ //
+
+ //javadoc: meanStdDev(src, mean, stddev, mask)
+ public static void meanStdDev(Mat src, MatOfDouble mean, MatOfDouble stddev, Mat mask)
+ {
+ Mat mean_mat = mean;
+ Mat stddev_mat = stddev;
+ meanStdDev_0(src.nativeObj, mean_mat.nativeObj, stddev_mat.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: meanStdDev(src, mean, stddev)
+ public static void meanStdDev(Mat src, MatOfDouble mean, MatOfDouble stddev)
+ {
+ Mat mean_mat = mean;
+ Mat stddev_mat = stddev;
+ meanStdDev_1(src.nativeObj, mean_mat.nativeObj, stddev_mat.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void merge(vector_Mat mv, Mat& dst)
+ //
+
+ //javadoc: merge(mv, dst)
+ public static void merge(List mv, Mat dst)
+ {
+ Mat mv_mat = Converters.vector_Mat_to_Mat(mv);
+ merge_0(mv_mat.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void min(Mat src1, Mat src2, Mat& dst)
+ //
+
+ //javadoc: min(src1, src2, dst)
+ public static void min(Mat src1, Mat src2, Mat dst)
+ {
+
+ min_0(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void min(Mat src1, Scalar src2, Mat& dst)
+ //
+
+ //javadoc: min(src1, src2, dst)
+ public static void min(Mat src1, Scalar src2, Mat dst)
+ {
+
+ min_1(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void mixChannels(vector_Mat src, vector_Mat dst, vector_int fromTo)
+ //
+
+ //javadoc: mixChannels(src, dst, fromTo)
+ public static void mixChannels(List src, List dst, MatOfInt fromTo)
+ {
+ Mat src_mat = Converters.vector_Mat_to_Mat(src);
+ Mat dst_mat = Converters.vector_Mat_to_Mat(dst);
+ Mat fromTo_mat = fromTo;
+ mixChannels_0(src_mat.nativeObj, dst_mat.nativeObj, fromTo_mat.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void mulSpectrums(Mat a, Mat b, Mat& c, int flags, bool conjB = false)
+ //
+
+ //javadoc: mulSpectrums(a, b, c, flags, conjB)
+ public static void mulSpectrums(Mat a, Mat b, Mat c, int flags, boolean conjB)
+ {
+
+ mulSpectrums_0(a.nativeObj, b.nativeObj, c.nativeObj, flags, conjB);
+
+ return;
+ }
+
+ //javadoc: mulSpectrums(a, b, c, flags)
+ public static void mulSpectrums(Mat a, Mat b, Mat c, int flags)
+ {
+
+ mulSpectrums_1(a.nativeObj, b.nativeObj, c.nativeObj, flags);
+
+ return;
+ }
+
+
+ //
+ // C++: void mulTransposed(Mat src, Mat& dst, bool aTa, Mat delta = Mat(), double scale = 1, int dtype = -1)
+ //
+
+ //javadoc: mulTransposed(src, dst, aTa, delta, scale, dtype)
+ public static void mulTransposed(Mat src, Mat dst, boolean aTa, Mat delta, double scale, int dtype)
+ {
+
+ mulTransposed_0(src.nativeObj, dst.nativeObj, aTa, delta.nativeObj, scale, dtype);
+
+ return;
+ }
+
+ //javadoc: mulTransposed(src, dst, aTa, delta, scale)
+ public static void mulTransposed(Mat src, Mat dst, boolean aTa, Mat delta, double scale)
+ {
+
+ mulTransposed_1(src.nativeObj, dst.nativeObj, aTa, delta.nativeObj, scale);
+
+ return;
+ }
+
+ //javadoc: mulTransposed(src, dst, aTa)
+ public static void mulTransposed(Mat src, Mat dst, boolean aTa)
+ {
+
+ mulTransposed_2(src.nativeObj, dst.nativeObj, aTa);
+
+ return;
+ }
+
+
+ //
+ // C++: void multiply(Mat src1, Mat src2, Mat& dst, double scale = 1, int dtype = -1)
+ //
+
+ //javadoc: multiply(src1, src2, dst, scale, dtype)
+ public static void multiply(Mat src1, Mat src2, Mat dst, double scale, int dtype)
+ {
+
+ multiply_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, scale, dtype);
+
+ return;
+ }
+
+ //javadoc: multiply(src1, src2, dst, scale)
+ public static void multiply(Mat src1, Mat src2, Mat dst, double scale)
+ {
+
+ multiply_1(src1.nativeObj, src2.nativeObj, dst.nativeObj, scale);
+
+ return;
+ }
+
+ //javadoc: multiply(src1, src2, dst)
+ public static void multiply(Mat src1, Mat src2, Mat dst)
+ {
+
+ multiply_2(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void multiply(Mat src1, Scalar src2, Mat& dst, double scale = 1, int dtype = -1)
+ //
+
+ //javadoc: multiply(src1, src2, dst, scale, dtype)
+ public static void multiply(Mat src1, Scalar src2, Mat dst, double scale, int dtype)
+ {
+
+ multiply_3(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, scale, dtype);
+
+ return;
+ }
+
+ //javadoc: multiply(src1, src2, dst, scale)
+ public static void multiply(Mat src1, Scalar src2, Mat dst, double scale)
+ {
+
+ multiply_4(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, scale);
+
+ return;
+ }
+
+ //javadoc: multiply(src1, src2, dst)
+ public static void multiply(Mat src1, Scalar src2, Mat dst)
+ {
+
+ multiply_5(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void normalize(Mat src, Mat& dst, double alpha = 1, double beta = 0, int norm_type = NORM_L2, int dtype = -1, Mat mask = Mat())
+ //
+
+ //javadoc: normalize(src, dst, alpha, beta, norm_type, dtype, mask)
+ public static void normalize(Mat src, Mat dst, double alpha, double beta, int norm_type, int dtype, Mat mask)
+ {
+
+ normalize_0(src.nativeObj, dst.nativeObj, alpha, beta, norm_type, dtype, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: normalize(src, dst, alpha, beta, norm_type, dtype)
+ public static void normalize(Mat src, Mat dst, double alpha, double beta, int norm_type, int dtype)
+ {
+
+ normalize_1(src.nativeObj, dst.nativeObj, alpha, beta, norm_type, dtype);
+
+ return;
+ }
+
+ //javadoc: normalize(src, dst, alpha, beta, norm_type)
+ public static void normalize(Mat src, Mat dst, double alpha, double beta, int norm_type)
+ {
+
+ normalize_2(src.nativeObj, dst.nativeObj, alpha, beta, norm_type);
+
+ return;
+ }
+
+ //javadoc: normalize(src, dst)
+ public static void normalize(Mat src, Mat dst)
+ {
+
+ normalize_3(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void patchNaNs(Mat& a, double val = 0)
+ //
+
+ //javadoc: patchNaNs(a, val)
+ public static void patchNaNs(Mat a, double val)
+ {
+
+ patchNaNs_0(a.nativeObj, val);
+
+ return;
+ }
+
+ //javadoc: patchNaNs(a)
+ public static void patchNaNs(Mat a)
+ {
+
+ patchNaNs_1(a.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void perspectiveTransform(Mat src, Mat& dst, Mat m)
+ //
+
+ //javadoc: perspectiveTransform(src, dst, m)
+ public static void perspectiveTransform(Mat src, Mat dst, Mat m)
+ {
+
+ perspectiveTransform_0(src.nativeObj, dst.nativeObj, m.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void phase(Mat x, Mat y, Mat& angle, bool angleInDegrees = false)
+ //
+
+ //javadoc: phase(x, y, angle, angleInDegrees)
+ public static void phase(Mat x, Mat y, Mat angle, boolean angleInDegrees)
+ {
+
+ phase_0(x.nativeObj, y.nativeObj, angle.nativeObj, angleInDegrees);
+
+ return;
+ }
+
+ //javadoc: phase(x, y, angle)
+ public static void phase(Mat x, Mat y, Mat angle)
+ {
+
+ phase_1(x.nativeObj, y.nativeObj, angle.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void polarToCart(Mat magnitude, Mat angle, Mat& x, Mat& y, bool angleInDegrees = false)
+ //
+
+ //javadoc: polarToCart(magnitude, angle, x, y, angleInDegrees)
+ public static void polarToCart(Mat magnitude, Mat angle, Mat x, Mat y, boolean angleInDegrees)
+ {
+
+ polarToCart_0(magnitude.nativeObj, angle.nativeObj, x.nativeObj, y.nativeObj, angleInDegrees);
+
+ return;
+ }
+
+ //javadoc: polarToCart(magnitude, angle, x, y)
+ public static void polarToCart(Mat magnitude, Mat angle, Mat x, Mat y)
+ {
+
+ polarToCart_1(magnitude.nativeObj, angle.nativeObj, x.nativeObj, y.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void pow(Mat src, double power, Mat& dst)
+ //
+
+ //javadoc: pow(src, power, dst)
+ public static void pow(Mat src, double power, Mat dst)
+ {
+
+ pow_0(src.nativeObj, power, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void randShuffle(Mat& dst, double iterFactor = 1., RNG* rng = 0)
+ //
+
+ //javadoc: randShuffle(dst, iterFactor)
+ public static void randShuffle(Mat dst, double iterFactor)
+ {
+
+ randShuffle_0(dst.nativeObj, iterFactor);
+
+ return;
+ }
+
+ //javadoc: randShuffle(dst)
+ public static void randShuffle(Mat dst)
+ {
+
+ randShuffle_1(dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void randn(Mat& dst, double mean, double stddev)
+ //
+
+ //javadoc: randn(dst, mean, stddev)
+ public static void randn(Mat dst, double mean, double stddev)
+ {
+
+ randn_0(dst.nativeObj, mean, stddev);
+
+ return;
+ }
+
+
+ //
+ // C++: void randu(Mat& dst, double low, double high)
+ //
+
+ //javadoc: randu(dst, low, high)
+ public static void randu(Mat dst, double low, double high)
+ {
+
+ randu_0(dst.nativeObj, low, high);
+
+ return;
+ }
+
+
+ //
+ // C++: void reduce(Mat src, Mat& dst, int dim, int rtype, int dtype = -1)
+ //
+
+ //javadoc: reduce(src, dst, dim, rtype, dtype)
+ public static void reduce(Mat src, Mat dst, int dim, int rtype, int dtype)
+ {
+
+ reduce_0(src.nativeObj, dst.nativeObj, dim, rtype, dtype);
+
+ return;
+ }
+
+ //javadoc: reduce(src, dst, dim, rtype)
+ public static void reduce(Mat src, Mat dst, int dim, int rtype)
+ {
+
+ reduce_1(src.nativeObj, dst.nativeObj, dim, rtype);
+
+ return;
+ }
+
+
+ //
+ // C++: void repeat(Mat src, int ny, int nx, Mat& dst)
+ //
+
+ //javadoc: repeat(src, ny, nx, dst)
+ public static void repeat(Mat src, int ny, int nx, Mat dst)
+ {
+
+ repeat_0(src.nativeObj, ny, nx, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void rotate(Mat src, Mat& dst, int rotateCode)
+ //
+
+ //javadoc: rotate(src, dst, rotateCode)
+ public static void rotate(Mat src, Mat dst, int rotateCode)
+ {
+
+ rotate_0(src.nativeObj, dst.nativeObj, rotateCode);
+
+ return;
+ }
+
+
+ //
+ // C++: void scaleAdd(Mat src1, double alpha, Mat src2, Mat& dst)
+ //
+
+ //javadoc: scaleAdd(src1, alpha, src2, dst)
+ public static void scaleAdd(Mat src1, double alpha, Mat src2, Mat dst)
+ {
+
+ scaleAdd_0(src1.nativeObj, alpha, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void setErrorVerbosity(bool verbose)
+ //
+
+ //javadoc: setErrorVerbosity(verbose)
+ public static void setErrorVerbosity(boolean verbose)
+ {
+
+ setErrorVerbosity_0(verbose);
+
+ return;
+ }
+
+
+ //
+ // C++: void setIdentity(Mat& mtx, Scalar s = Scalar(1))
+ //
+
+ //javadoc: setIdentity(mtx, s)
+ public static void setIdentity(Mat mtx, Scalar s)
+ {
+
+ setIdentity_0(mtx.nativeObj, s.val[0], s.val[1], s.val[2], s.val[3]);
+
+ return;
+ }
+
+ //javadoc: setIdentity(mtx)
+ public static void setIdentity(Mat mtx)
+ {
+
+ setIdentity_1(mtx.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void setNumThreads(int nthreads)
+ //
+
+ //javadoc: setNumThreads(nthreads)
+ public static void setNumThreads(int nthreads)
+ {
+
+ setNumThreads_0(nthreads);
+
+ return;
+ }
+
+
+ //
+ // C++: void setRNGSeed(int seed)
+ //
+
+ //javadoc: setRNGSeed(seed)
+ public static void setRNGSeed(int seed)
+ {
+
+ setRNGSeed_0(seed);
+
+ return;
+ }
+
+
+ //
+ // C++: void sort(Mat src, Mat& dst, int flags)
+ //
+
+ //javadoc: sort(src, dst, flags)
+ public static void sort(Mat src, Mat dst, int flags)
+ {
+
+ sort_0(src.nativeObj, dst.nativeObj, flags);
+
+ return;
+ }
+
+
+ //
+ // C++: void sortIdx(Mat src, Mat& dst, int flags)
+ //
+
+ //javadoc: sortIdx(src, dst, flags)
+ public static void sortIdx(Mat src, Mat dst, int flags)
+ {
+
+ sortIdx_0(src.nativeObj, dst.nativeObj, flags);
+
+ return;
+ }
+
+
+ //
+ // C++: void split(Mat m, vector_Mat& mv)
+ //
+
+ //javadoc: split(m, mv)
+ public static void split(Mat m, List mv)
+ {
+ Mat mv_mat = new Mat();
+ split_0(m.nativeObj, mv_mat.nativeObj);
+ Converters.Mat_to_vector_Mat(mv_mat, mv);
+ mv_mat.release();
+ return;
+ }
+
+
+ //
+ // C++: void sqrt(Mat src, Mat& dst)
+ //
+
+ //javadoc: sqrt(src, dst)
+ public static void sqrt(Mat src, Mat dst)
+ {
+
+ sqrt_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void subtract(Mat src1, Mat src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ //
+
+ //javadoc: subtract(src1, src2, dst, mask, dtype)
+ public static void subtract(Mat src1, Mat src2, Mat dst, Mat mask, int dtype)
+ {
+
+ subtract_0(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj, dtype);
+
+ return;
+ }
+
+ //javadoc: subtract(src1, src2, dst, mask)
+ public static void subtract(Mat src1, Mat src2, Mat dst, Mat mask)
+ {
+
+ subtract_1(src1.nativeObj, src2.nativeObj, dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: subtract(src1, src2, dst)
+ public static void subtract(Mat src1, Mat src2, Mat dst)
+ {
+
+ subtract_2(src1.nativeObj, src2.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void subtract(Mat src1, Scalar src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ //
+
+ //javadoc: subtract(src1, src2, dst, mask, dtype)
+ public static void subtract(Mat src1, Scalar src2, Mat dst, Mat mask, int dtype)
+ {
+
+ subtract_3(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, mask.nativeObj, dtype);
+
+ return;
+ }
+
+ //javadoc: subtract(src1, src2, dst, mask)
+ public static void subtract(Mat src1, Scalar src2, Mat dst, Mat mask)
+ {
+
+ subtract_4(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //javadoc: subtract(src1, src2, dst)
+ public static void subtract(Mat src1, Scalar src2, Mat dst)
+ {
+
+ subtract_5(src1.nativeObj, src2.val[0], src2.val[1], src2.val[2], src2.val[3], dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void transform(Mat src, Mat& dst, Mat m)
+ //
+
+ //javadoc: transform(src, dst, m)
+ public static void transform(Mat src, Mat dst, Mat m)
+ {
+
+ transform_0(src.nativeObj, dst.nativeObj, m.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void transpose(Mat src, Mat& dst)
+ //
+
+ //javadoc: transpose(src, dst)
+ public static void transpose(Mat src, Mat dst)
+ {
+
+ transpose_0(src.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void vconcat(vector_Mat src, Mat& dst)
+ //
+
+ //javadoc: vconcat(src, dst)
+ public static void vconcat(List src, Mat dst)
+ {
+ Mat src_mat = Converters.vector_Mat_to_Mat(src);
+ vconcat_0(src_mat.nativeObj, dst.nativeObj);
+
+ return;
+ }
+
+
+ //
+ // C++: void setUseIPP(bool flag)
+ //
+
+ //javadoc: setUseIPP(flag)
+ public static void setUseIPP(boolean flag)
+ {
+
+ setUseIPP_0(flag);
+
+ return;
+ }
+
+// manual port
+public static class MinMaxLocResult {
+ public double minVal;
+ public double maxVal;
+ public Point minLoc;
+ public Point maxLoc;
+
+
+ public MinMaxLocResult() {
+ minVal=0; maxVal=0;
+ minLoc=new Point();
+ maxLoc=new Point();
+ }
+}
+
+
+// C++: minMaxLoc(Mat src, double* minVal, double* maxVal=0, Point* minLoc=0, Point* maxLoc=0, InputArray mask=noArray())
+
+
+//javadoc: minMaxLoc(src, mask)
+public static MinMaxLocResult minMaxLoc(Mat src, Mat mask) {
+ MinMaxLocResult res = new MinMaxLocResult();
+ long maskNativeObj=0;
+ if (mask != null) {
+ maskNativeObj=mask.nativeObj;
+ }
+ double resarr[] = n_minMaxLocManual(src.nativeObj, maskNativeObj);
+ res.minVal=resarr[0];
+ res.maxVal=resarr[1];
+ res.minLoc.x=resarr[2];
+ res.minLoc.y=resarr[3];
+ res.maxLoc.x=resarr[4];
+ res.maxLoc.y=resarr[5];
+ return res;
+}
+
+
+//javadoc: minMaxLoc(src)
+public static MinMaxLocResult minMaxLoc(Mat src) {
+ return minMaxLoc(src, null);
+}
+
+
+ // C++: Scalar mean(Mat src, Mat mask = Mat())
+ private static native double[] mean_0(long src_nativeObj, long mask_nativeObj);
+ private static native double[] mean_1(long src_nativeObj);
+
+ // C++: Scalar sum(Mat src)
+ private static native double[] sumElems_0(long src_nativeObj);
+
+ // C++: Scalar trace(Mat mtx)
+ private static native double[] trace_0(long mtx_nativeObj);
+
+ // C++: String getBuildInformation()
+ private static native String getBuildInformation_0();
+
+ // C++: bool checkRange(Mat a, bool quiet = true, _hidden_ * pos = 0, double minVal = -DBL_MAX, double maxVal = DBL_MAX)
+ private static native boolean checkRange_0(long a_nativeObj, boolean quiet, double minVal, double maxVal);
+ private static native boolean checkRange_1(long a_nativeObj);
+
+ // C++: bool eigen(Mat src, Mat& eigenvalues, Mat& eigenvectors = Mat())
+ private static native boolean eigen_0(long src_nativeObj, long eigenvalues_nativeObj, long eigenvectors_nativeObj);
+ private static native boolean eigen_1(long src_nativeObj, long eigenvalues_nativeObj);
+
+ // C++: bool solve(Mat src1, Mat src2, Mat& dst, int flags = DECOMP_LU)
+ private static native boolean solve_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, int flags);
+ private static native boolean solve_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: bool useIPP()
+ private static native boolean useIPP_0();
+
+ // C++: double Mahalanobis(Mat v1, Mat v2, Mat icovar)
+ private static native double Mahalanobis_0(long v1_nativeObj, long v2_nativeObj, long icovar_nativeObj);
+
+ // C++: double PSNR(Mat src1, Mat src2)
+ private static native double PSNR_0(long src1_nativeObj, long src2_nativeObj);
+
+ // C++: double determinant(Mat mtx)
+ private static native double determinant_0(long mtx_nativeObj);
+
+ // C++: double getTickFrequency()
+ private static native double getTickFrequency_0();
+
+ // C++: double invert(Mat src, Mat& dst, int flags = DECOMP_LU)
+ private static native double invert_0(long src_nativeObj, long dst_nativeObj, int flags);
+ private static native double invert_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: double kmeans(Mat data, int K, Mat& bestLabels, TermCriteria criteria, int attempts, int flags, Mat& centers = Mat())
+ private static native double kmeans_0(long data_nativeObj, int K, long bestLabels_nativeObj, int criteria_type, int criteria_maxCount, double criteria_epsilon, int attempts, int flags, long centers_nativeObj);
+ private static native double kmeans_1(long data_nativeObj, int K, long bestLabels_nativeObj, int criteria_type, int criteria_maxCount, double criteria_epsilon, int attempts, int flags);
+
+ // C++: double norm(Mat src1, Mat src2, int normType = NORM_L2, Mat mask = Mat())
+ private static native double norm_0(long src1_nativeObj, long src2_nativeObj, int normType, long mask_nativeObj);
+ private static native double norm_1(long src1_nativeObj, long src2_nativeObj, int normType);
+ private static native double norm_2(long src1_nativeObj, long src2_nativeObj);
+
+ // C++: double norm(Mat src1, int normType = NORM_L2, Mat mask = Mat())
+ private static native double norm_3(long src1_nativeObj, int normType, long mask_nativeObj);
+ private static native double norm_4(long src1_nativeObj, int normType);
+ private static native double norm_5(long src1_nativeObj);
+
+ // C++: double solvePoly(Mat coeffs, Mat& roots, int maxIters = 300)
+ private static native double solvePoly_0(long coeffs_nativeObj, long roots_nativeObj, int maxIters);
+ private static native double solvePoly_1(long coeffs_nativeObj, long roots_nativeObj);
+
+ // C++: float cubeRoot(float val)
+ private static native float cubeRoot_0(float val);
+
+ // C++: float fastAtan2(float y, float x)
+ private static native float fastAtan2_0(float y, float x);
+
+ // C++: int borderInterpolate(int p, int len, int borderType)
+ private static native int borderInterpolate_0(int p, int len, int borderType);
+
+ // C++: int countNonZero(Mat src)
+ private static native int countNonZero_0(long src_nativeObj);
+
+ // C++: int getNumThreads()
+ private static native int getNumThreads_0();
+
+ // C++: int getNumberOfCPUs()
+ private static native int getNumberOfCPUs_0();
+
+ // C++: int getOptimalDFTSize(int vecsize)
+ private static native int getOptimalDFTSize_0(int vecsize);
+
+ // C++: int getThreadNum()
+ private static native int getThreadNum_0();
+
+ // C++: int solveCubic(Mat coeffs, Mat& roots)
+ private static native int solveCubic_0(long coeffs_nativeObj, long roots_nativeObj);
+
+ // C++: int64 getCPUTickCount()
+ private static native long getCPUTickCount_0();
+
+ // C++: int64 getTickCount()
+ private static native long getTickCount_0();
+
+ // C++: void LUT(Mat src, Mat lut, Mat& dst)
+ private static native void LUT_0(long src_nativeObj, long lut_nativeObj, long dst_nativeObj);
+
+ // C++: void PCABackProject(Mat data, Mat mean, Mat eigenvectors, Mat& result)
+ private static native void PCABackProject_0(long data_nativeObj, long mean_nativeObj, long eigenvectors_nativeObj, long result_nativeObj);
+
+ // C++: void PCACompute(Mat data, Mat& mean, Mat& eigenvectors, double retainedVariance)
+ private static native void PCACompute_0(long data_nativeObj, long mean_nativeObj, long eigenvectors_nativeObj, double retainedVariance);
+
+ // C++: void PCACompute(Mat data, Mat& mean, Mat& eigenvectors, int maxComponents = 0)
+ private static native void PCACompute_1(long data_nativeObj, long mean_nativeObj, long eigenvectors_nativeObj, int maxComponents);
+ private static native void PCACompute_2(long data_nativeObj, long mean_nativeObj, long eigenvectors_nativeObj);
+
+ // C++: void PCAProject(Mat data, Mat mean, Mat eigenvectors, Mat& result)
+ private static native void PCAProject_0(long data_nativeObj, long mean_nativeObj, long eigenvectors_nativeObj, long result_nativeObj);
+
+ // C++: void SVBackSubst(Mat w, Mat u, Mat vt, Mat rhs, Mat& dst)
+ private static native void SVBackSubst_0(long w_nativeObj, long u_nativeObj, long vt_nativeObj, long rhs_nativeObj, long dst_nativeObj);
+
+ // C++: void SVDecomp(Mat src, Mat& w, Mat& u, Mat& vt, int flags = 0)
+ private static native void SVDecomp_0(long src_nativeObj, long w_nativeObj, long u_nativeObj, long vt_nativeObj, int flags);
+ private static native void SVDecomp_1(long src_nativeObj, long w_nativeObj, long u_nativeObj, long vt_nativeObj);
+
+ // C++: void absdiff(Mat src1, Mat src2, Mat& dst)
+ private static native void absdiff_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void absdiff(Mat src1, Scalar src2, Mat& dst)
+ private static native void absdiff_1(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void add(Mat src1, Mat src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ private static native void add_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj, int dtype);
+ private static native void add_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj);
+ private static native void add_2(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void add(Mat src1, Scalar src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ private static native void add_3(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, long mask_nativeObj, int dtype);
+ private static native void add_4(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, long mask_nativeObj);
+ private static native void add_5(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void addWeighted(Mat src1, double alpha, Mat src2, double beta, double gamma, Mat& dst, int dtype = -1)
+ private static native void addWeighted_0(long src1_nativeObj, double alpha, long src2_nativeObj, double beta, double gamma, long dst_nativeObj, int dtype);
+ private static native void addWeighted_1(long src1_nativeObj, double alpha, long src2_nativeObj, double beta, double gamma, long dst_nativeObj);
+
+ // C++: void batchDistance(Mat src1, Mat src2, Mat& dist, int dtype, Mat& nidx, int normType = NORM_L2, int K = 0, Mat mask = Mat(), int update = 0, bool crosscheck = false)
+ private static native void batchDistance_0(long src1_nativeObj, long src2_nativeObj, long dist_nativeObj, int dtype, long nidx_nativeObj, int normType, int K, long mask_nativeObj, int update, boolean crosscheck);
+ private static native void batchDistance_1(long src1_nativeObj, long src2_nativeObj, long dist_nativeObj, int dtype, long nidx_nativeObj, int normType, int K);
+ private static native void batchDistance_2(long src1_nativeObj, long src2_nativeObj, long dist_nativeObj, int dtype, long nidx_nativeObj);
+
+ // C++: void bitwise_and(Mat src1, Mat src2, Mat& dst, Mat mask = Mat())
+ private static native void bitwise_and_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj);
+ private static native void bitwise_and_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void bitwise_not(Mat src, Mat& dst, Mat mask = Mat())
+ private static native void bitwise_not_0(long src_nativeObj, long dst_nativeObj, long mask_nativeObj);
+ private static native void bitwise_not_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void bitwise_or(Mat src1, Mat src2, Mat& dst, Mat mask = Mat())
+ private static native void bitwise_or_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj);
+ private static native void bitwise_or_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void bitwise_xor(Mat src1, Mat src2, Mat& dst, Mat mask = Mat())
+ private static native void bitwise_xor_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj);
+ private static native void bitwise_xor_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void calcCovarMatrix(Mat samples, Mat& covar, Mat& mean, int flags, int ctype = CV_64F)
+ private static native void calcCovarMatrix_0(long samples_nativeObj, long covar_nativeObj, long mean_nativeObj, int flags, int ctype);
+ private static native void calcCovarMatrix_1(long samples_nativeObj, long covar_nativeObj, long mean_nativeObj, int flags);
+
+ // C++: void cartToPolar(Mat x, Mat y, Mat& magnitude, Mat& angle, bool angleInDegrees = false)
+ private static native void cartToPolar_0(long x_nativeObj, long y_nativeObj, long magnitude_nativeObj, long angle_nativeObj, boolean angleInDegrees);
+ private static native void cartToPolar_1(long x_nativeObj, long y_nativeObj, long magnitude_nativeObj, long angle_nativeObj);
+
+ // C++: void compare(Mat src1, Mat src2, Mat& dst, int cmpop)
+ private static native void compare_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, int cmpop);
+
+ // C++: void compare(Mat src1, Scalar src2, Mat& dst, int cmpop)
+ private static native void compare_1(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, int cmpop);
+
+ // C++: void completeSymm(Mat& mtx, bool lowerToUpper = false)
+ private static native void completeSymm_0(long mtx_nativeObj, boolean lowerToUpper);
+ private static native void completeSymm_1(long mtx_nativeObj);
+
+ // C++: void convertFp16(Mat src, Mat& dst)
+ private static native void convertFp16_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void convertScaleAbs(Mat src, Mat& dst, double alpha = 1, double beta = 0)
+ private static native void convertScaleAbs_0(long src_nativeObj, long dst_nativeObj, double alpha, double beta);
+ private static native void convertScaleAbs_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void copyMakeBorder(Mat src, Mat& dst, int top, int bottom, int left, int right, int borderType, Scalar value = Scalar())
+ private static native void copyMakeBorder_0(long src_nativeObj, long dst_nativeObj, int top, int bottom, int left, int right, int borderType, double value_val0, double value_val1, double value_val2, double value_val3);
+ private static native void copyMakeBorder_1(long src_nativeObj, long dst_nativeObj, int top, int bottom, int left, int right, int borderType);
+
+ // C++: void dct(Mat src, Mat& dst, int flags = 0)
+ private static native void dct_0(long src_nativeObj, long dst_nativeObj, int flags);
+ private static native void dct_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void dft(Mat src, Mat& dst, int flags = 0, int nonzeroRows = 0)
+ private static native void dft_0(long src_nativeObj, long dst_nativeObj, int flags, int nonzeroRows);
+ private static native void dft_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void divide(Mat src1, Mat src2, Mat& dst, double scale = 1, int dtype = -1)
+ private static native void divide_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, double scale, int dtype);
+ private static native void divide_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, double scale);
+ private static native void divide_2(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void divide(Mat src1, Scalar src2, Mat& dst, double scale = 1, int dtype = -1)
+ private static native void divide_3(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, double scale, int dtype);
+ private static native void divide_4(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, double scale);
+ private static native void divide_5(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void divide(double scale, Mat src2, Mat& dst, int dtype = -1)
+ private static native void divide_6(double scale, long src2_nativeObj, long dst_nativeObj, int dtype);
+ private static native void divide_7(double scale, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void exp(Mat src, Mat& dst)
+ private static native void exp_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void extractChannel(Mat src, Mat& dst, int coi)
+ private static native void extractChannel_0(long src_nativeObj, long dst_nativeObj, int coi);
+
+ // C++: void findNonZero(Mat src, Mat& idx)
+ private static native void findNonZero_0(long src_nativeObj, long idx_nativeObj);
+
+ // C++: void flip(Mat src, Mat& dst, int flipCode)
+ private static native void flip_0(long src_nativeObj, long dst_nativeObj, int flipCode);
+
+ // C++: void gemm(Mat src1, Mat src2, double alpha, Mat src3, double beta, Mat& dst, int flags = 0)
+ private static native void gemm_0(long src1_nativeObj, long src2_nativeObj, double alpha, long src3_nativeObj, double beta, long dst_nativeObj, int flags);
+ private static native void gemm_1(long src1_nativeObj, long src2_nativeObj, double alpha, long src3_nativeObj, double beta, long dst_nativeObj);
+
+ // C++: void hconcat(vector_Mat src, Mat& dst)
+ private static native void hconcat_0(long src_mat_nativeObj, long dst_nativeObj);
+
+ // C++: void idct(Mat src, Mat& dst, int flags = 0)
+ private static native void idct_0(long src_nativeObj, long dst_nativeObj, int flags);
+ private static native void idct_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void idft(Mat src, Mat& dst, int flags = 0, int nonzeroRows = 0)
+ private static native void idft_0(long src_nativeObj, long dst_nativeObj, int flags, int nonzeroRows);
+ private static native void idft_1(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void inRange(Mat src, Scalar lowerb, Scalar upperb, Mat& dst)
+ private static native void inRange_0(long src_nativeObj, double lowerb_val0, double lowerb_val1, double lowerb_val2, double lowerb_val3, double upperb_val0, double upperb_val1, double upperb_val2, double upperb_val3, long dst_nativeObj);
+
+ // C++: void insertChannel(Mat src, Mat& dst, int coi)
+ private static native void insertChannel_0(long src_nativeObj, long dst_nativeObj, int coi);
+
+ // C++: void log(Mat src, Mat& dst)
+ private static native void log_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void magnitude(Mat x, Mat y, Mat& magnitude)
+ private static native void magnitude_0(long x_nativeObj, long y_nativeObj, long magnitude_nativeObj);
+
+ // C++: void max(Mat src1, Mat src2, Mat& dst)
+ private static native void max_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void max(Mat src1, Scalar src2, Mat& dst)
+ private static native void max_1(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void meanStdDev(Mat src, vector_double& mean, vector_double& stddev, Mat mask = Mat())
+ private static native void meanStdDev_0(long src_nativeObj, long mean_mat_nativeObj, long stddev_mat_nativeObj, long mask_nativeObj);
+ private static native void meanStdDev_1(long src_nativeObj, long mean_mat_nativeObj, long stddev_mat_nativeObj);
+
+ // C++: void merge(vector_Mat mv, Mat& dst)
+ private static native void merge_0(long mv_mat_nativeObj, long dst_nativeObj);
+
+ // C++: void min(Mat src1, Mat src2, Mat& dst)
+ private static native void min_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void min(Mat src1, Scalar src2, Mat& dst)
+ private static native void min_1(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void mixChannels(vector_Mat src, vector_Mat dst, vector_int fromTo)
+ private static native void mixChannels_0(long src_mat_nativeObj, long dst_mat_nativeObj, long fromTo_mat_nativeObj);
+
+ // C++: void mulSpectrums(Mat a, Mat b, Mat& c, int flags, bool conjB = false)
+ private static native void mulSpectrums_0(long a_nativeObj, long b_nativeObj, long c_nativeObj, int flags, boolean conjB);
+ private static native void mulSpectrums_1(long a_nativeObj, long b_nativeObj, long c_nativeObj, int flags);
+
+ // C++: void mulTransposed(Mat src, Mat& dst, bool aTa, Mat delta = Mat(), double scale = 1, int dtype = -1)
+ private static native void mulTransposed_0(long src_nativeObj, long dst_nativeObj, boolean aTa, long delta_nativeObj, double scale, int dtype);
+ private static native void mulTransposed_1(long src_nativeObj, long dst_nativeObj, boolean aTa, long delta_nativeObj, double scale);
+ private static native void mulTransposed_2(long src_nativeObj, long dst_nativeObj, boolean aTa);
+
+ // C++: void multiply(Mat src1, Mat src2, Mat& dst, double scale = 1, int dtype = -1)
+ private static native void multiply_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, double scale, int dtype);
+ private static native void multiply_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, double scale);
+ private static native void multiply_2(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void multiply(Mat src1, Scalar src2, Mat& dst, double scale = 1, int dtype = -1)
+ private static native void multiply_3(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, double scale, int dtype);
+ private static native void multiply_4(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, double scale);
+ private static native void multiply_5(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void normalize(Mat src, Mat& dst, double alpha = 1, double beta = 0, int norm_type = NORM_L2, int dtype = -1, Mat mask = Mat())
+ private static native void normalize_0(long src_nativeObj, long dst_nativeObj, double alpha, double beta, int norm_type, int dtype, long mask_nativeObj);
+ private static native void normalize_1(long src_nativeObj, long dst_nativeObj, double alpha, double beta, int norm_type, int dtype);
+ private static native void normalize_2(long src_nativeObj, long dst_nativeObj, double alpha, double beta, int norm_type);
+ private static native void normalize_3(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void patchNaNs(Mat& a, double val = 0)
+ private static native void patchNaNs_0(long a_nativeObj, double val);
+ private static native void patchNaNs_1(long a_nativeObj);
+
+ // C++: void perspectiveTransform(Mat src, Mat& dst, Mat m)
+ private static native void perspectiveTransform_0(long src_nativeObj, long dst_nativeObj, long m_nativeObj);
+
+ // C++: void phase(Mat x, Mat y, Mat& angle, bool angleInDegrees = false)
+ private static native void phase_0(long x_nativeObj, long y_nativeObj, long angle_nativeObj, boolean angleInDegrees);
+ private static native void phase_1(long x_nativeObj, long y_nativeObj, long angle_nativeObj);
+
+ // C++: void polarToCart(Mat magnitude, Mat angle, Mat& x, Mat& y, bool angleInDegrees = false)
+ private static native void polarToCart_0(long magnitude_nativeObj, long angle_nativeObj, long x_nativeObj, long y_nativeObj, boolean angleInDegrees);
+ private static native void polarToCart_1(long magnitude_nativeObj, long angle_nativeObj, long x_nativeObj, long y_nativeObj);
+
+ // C++: void pow(Mat src, double power, Mat& dst)
+ private static native void pow_0(long src_nativeObj, double power, long dst_nativeObj);
+
+ // C++: void randShuffle(Mat& dst, double iterFactor = 1., RNG* rng = 0)
+ private static native void randShuffle_0(long dst_nativeObj, double iterFactor);
+ private static native void randShuffle_1(long dst_nativeObj);
+
+ // C++: void randn(Mat& dst, double mean, double stddev)
+ private static native void randn_0(long dst_nativeObj, double mean, double stddev);
+
+ // C++: void randu(Mat& dst, double low, double high)
+ private static native void randu_0(long dst_nativeObj, double low, double high);
+
+ // C++: void reduce(Mat src, Mat& dst, int dim, int rtype, int dtype = -1)
+ private static native void reduce_0(long src_nativeObj, long dst_nativeObj, int dim, int rtype, int dtype);
+ private static native void reduce_1(long src_nativeObj, long dst_nativeObj, int dim, int rtype);
+
+ // C++: void repeat(Mat src, int ny, int nx, Mat& dst)
+ private static native void repeat_0(long src_nativeObj, int ny, int nx, long dst_nativeObj);
+
+ // C++: void rotate(Mat src, Mat& dst, int rotateCode)
+ private static native void rotate_0(long src_nativeObj, long dst_nativeObj, int rotateCode);
+
+ // C++: void scaleAdd(Mat src1, double alpha, Mat src2, Mat& dst)
+ private static native void scaleAdd_0(long src1_nativeObj, double alpha, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void setErrorVerbosity(bool verbose)
+ private static native void setErrorVerbosity_0(boolean verbose);
+
+ // C++: void setIdentity(Mat& mtx, Scalar s = Scalar(1))
+ private static native void setIdentity_0(long mtx_nativeObj, double s_val0, double s_val1, double s_val2, double s_val3);
+ private static native void setIdentity_1(long mtx_nativeObj);
+
+ // C++: void setNumThreads(int nthreads)
+ private static native void setNumThreads_0(int nthreads);
+
+ // C++: void setRNGSeed(int seed)
+ private static native void setRNGSeed_0(int seed);
+
+ // C++: void sort(Mat src, Mat& dst, int flags)
+ private static native void sort_0(long src_nativeObj, long dst_nativeObj, int flags);
+
+ // C++: void sortIdx(Mat src, Mat& dst, int flags)
+ private static native void sortIdx_0(long src_nativeObj, long dst_nativeObj, int flags);
+
+ // C++: void split(Mat m, vector_Mat& mv)
+ private static native void split_0(long m_nativeObj, long mv_mat_nativeObj);
+
+ // C++: void sqrt(Mat src, Mat& dst)
+ private static native void sqrt_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void subtract(Mat src1, Mat src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ private static native void subtract_0(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj, int dtype);
+ private static native void subtract_1(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj, long mask_nativeObj);
+ private static native void subtract_2(long src1_nativeObj, long src2_nativeObj, long dst_nativeObj);
+
+ // C++: void subtract(Mat src1, Scalar src2, Mat& dst, Mat mask = Mat(), int dtype = -1)
+ private static native void subtract_3(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, long mask_nativeObj, int dtype);
+ private static native void subtract_4(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj, long mask_nativeObj);
+ private static native void subtract_5(long src1_nativeObj, double src2_val0, double src2_val1, double src2_val2, double src2_val3, long dst_nativeObj);
+
+ // C++: void transform(Mat src, Mat& dst, Mat m)
+ private static native void transform_0(long src_nativeObj, long dst_nativeObj, long m_nativeObj);
+
+ // C++: void transpose(Mat src, Mat& dst)
+ private static native void transpose_0(long src_nativeObj, long dst_nativeObj);
+
+ // C++: void vconcat(vector_Mat src, Mat& dst)
+ private static native void vconcat_0(long src_mat_nativeObj, long dst_nativeObj);
+
+ // C++: void setUseIPP(bool flag)
+ private static native void setUseIPP_0(boolean flag);
+private static native double[] n_minMaxLocManual(long src_nativeObj, long mask_nativeObj);
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/CvException.java b/openCVLibrary330/src/main/java/org/opencv/core/CvException.java
new file mode 100644
index 00000000000..e9241e68860
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/CvException.java
@@ -0,0 +1,15 @@
+package org.opencv.core;
+
+public class CvException extends RuntimeException {
+
+ private static final long serialVersionUID = 1L;
+
+ public CvException(String msg) {
+ super(msg);
+ }
+
+ @Override
+ public String toString() {
+ return "CvException [" + super.toString() + "]";
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/CvType.java b/openCVLibrary330/src/main/java/org/opencv/core/CvType.java
new file mode 100644
index 00000000000..748c1cd75c2
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/CvType.java
@@ -0,0 +1,136 @@
+package org.opencv.core;
+
+public final class CvType {
+
+ // type depth constants
+ public static final int
+ CV_8U = 0, CV_8S = 1,
+ CV_16U = 2, CV_16S = 3,
+ CV_32S = 4,
+ CV_32F = 5,
+ CV_64F = 6,
+ CV_USRTYPE1 = 7;
+
+ // predefined type constants
+ public static final int
+ CV_8UC1 = CV_8UC(1), CV_8UC2 = CV_8UC(2), CV_8UC3 = CV_8UC(3), CV_8UC4 = CV_8UC(4),
+ CV_8SC1 = CV_8SC(1), CV_8SC2 = CV_8SC(2), CV_8SC3 = CV_8SC(3), CV_8SC4 = CV_8SC(4),
+ CV_16UC1 = CV_16UC(1), CV_16UC2 = CV_16UC(2), CV_16UC3 = CV_16UC(3), CV_16UC4 = CV_16UC(4),
+ CV_16SC1 = CV_16SC(1), CV_16SC2 = CV_16SC(2), CV_16SC3 = CV_16SC(3), CV_16SC4 = CV_16SC(4),
+ CV_32SC1 = CV_32SC(1), CV_32SC2 = CV_32SC(2), CV_32SC3 = CV_32SC(3), CV_32SC4 = CV_32SC(4),
+ CV_32FC1 = CV_32FC(1), CV_32FC2 = CV_32FC(2), CV_32FC3 = CV_32FC(3), CV_32FC4 = CV_32FC(4),
+ CV_64FC1 = CV_64FC(1), CV_64FC2 = CV_64FC(2), CV_64FC3 = CV_64FC(3), CV_64FC4 = CV_64FC(4);
+
+ private static final int CV_CN_MAX = 512, CV_CN_SHIFT = 3, CV_DEPTH_MAX = (1 << CV_CN_SHIFT);
+
+ public static final int makeType(int depth, int channels) {
+ if (channels <= 0 || channels >= CV_CN_MAX) {
+ throw new java.lang.UnsupportedOperationException(
+ "Channels count should be 1.." + (CV_CN_MAX - 1));
+ }
+ if (depth < 0 || depth >= CV_DEPTH_MAX) {
+ throw new java.lang.UnsupportedOperationException(
+ "Data type depth should be 0.." + (CV_DEPTH_MAX - 1));
+ }
+ return (depth & (CV_DEPTH_MAX - 1)) + ((channels - 1) << CV_CN_SHIFT);
+ }
+
+ public static final int CV_8UC(int ch) {
+ return makeType(CV_8U, ch);
+ }
+
+ public static final int CV_8SC(int ch) {
+ return makeType(CV_8S, ch);
+ }
+
+ public static final int CV_16UC(int ch) {
+ return makeType(CV_16U, ch);
+ }
+
+ public static final int CV_16SC(int ch) {
+ return makeType(CV_16S, ch);
+ }
+
+ public static final int CV_32SC(int ch) {
+ return makeType(CV_32S, ch);
+ }
+
+ public static final int CV_32FC(int ch) {
+ return makeType(CV_32F, ch);
+ }
+
+ public static final int CV_64FC(int ch) {
+ return makeType(CV_64F, ch);
+ }
+
+ public static final int channels(int type) {
+ return (type >> CV_CN_SHIFT) + 1;
+ }
+
+ public static final int depth(int type) {
+ return type & (CV_DEPTH_MAX - 1);
+ }
+
+ public static final boolean isInteger(int type) {
+ return depth(type) < CV_32F;
+ }
+
+ public static final int ELEM_SIZE(int type) {
+ switch (depth(type)) {
+ case CV_8U:
+ case CV_8S:
+ return channels(type);
+ case CV_16U:
+ case CV_16S:
+ return 2 * channels(type);
+ case CV_32S:
+ case CV_32F:
+ return 4 * channels(type);
+ case CV_64F:
+ return 8 * channels(type);
+ default:
+ throw new java.lang.UnsupportedOperationException(
+ "Unsupported CvType value: " + type);
+ }
+ }
+
+ public static final String typeToString(int type) {
+ String s;
+ switch (depth(type)) {
+ case CV_8U:
+ s = "CV_8U";
+ break;
+ case CV_8S:
+ s = "CV_8S";
+ break;
+ case CV_16U:
+ s = "CV_16U";
+ break;
+ case CV_16S:
+ s = "CV_16S";
+ break;
+ case CV_32S:
+ s = "CV_32S";
+ break;
+ case CV_32F:
+ s = "CV_32F";
+ break;
+ case CV_64F:
+ s = "CV_64F";
+ break;
+ case CV_USRTYPE1:
+ s = "CV_USRTYPE1";
+ break;
+ default:
+ throw new java.lang.UnsupportedOperationException(
+ "Unsupported CvType value: " + type);
+ }
+
+ int ch = channels(type);
+ if (ch <= 4)
+ return s + "C" + ch;
+ else
+ return s + "C(" + ch + ")";
+ }
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/DMatch.java b/openCVLibrary330/src/main/java/org/opencv/core/DMatch.java
new file mode 100644
index 00000000000..db44d9a0a68
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/DMatch.java
@@ -0,0 +1,58 @@
+package org.opencv.core;
+
+//C++: class DMatch
+
+/**
+ * Structure for matching: query descriptor index, train descriptor index, train
+ * image index and distance between descriptors.
+ */
+public class DMatch {
+
+ /**
+ * Query descriptor index.
+ */
+ public int queryIdx;
+ /**
+ * Train descriptor index.
+ */
+ public int trainIdx;
+ /**
+ * Train image index.
+ */
+ public int imgIdx;
+
+ // javadoc: DMatch::distance
+ public float distance;
+
+ // javadoc: DMatch::DMatch()
+ public DMatch() {
+ this(-1, -1, Float.MAX_VALUE);
+ }
+
+ // javadoc: DMatch::DMatch(_queryIdx, _trainIdx, _distance)
+ public DMatch(int _queryIdx, int _trainIdx, float _distance) {
+ queryIdx = _queryIdx;
+ trainIdx = _trainIdx;
+ imgIdx = -1;
+ distance = _distance;
+ }
+
+ // javadoc: DMatch::DMatch(_queryIdx, _trainIdx, _imgIdx, _distance)
+ public DMatch(int _queryIdx, int _trainIdx, int _imgIdx, float _distance) {
+ queryIdx = _queryIdx;
+ trainIdx = _trainIdx;
+ imgIdx = _imgIdx;
+ distance = _distance;
+ }
+
+ public boolean lessThan(DMatch it) {
+ return distance < it.distance;
+ }
+
+ @Override
+ public String toString() {
+ return "DMatch [queryIdx=" + queryIdx + ", trainIdx=" + trainIdx
+ + ", imgIdx=" + imgIdx + ", distance=" + distance + "]";
+ }
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/KeyPoint.java b/openCVLibrary330/src/main/java/org/opencv/core/KeyPoint.java
new file mode 100644
index 00000000000..de5b2151ff7
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/KeyPoint.java
@@ -0,0 +1,83 @@
+package org.opencv.core;
+
+import org.opencv.core.Point;
+
+//javadoc: KeyPoint
+public class KeyPoint {
+
+ /**
+ * Coordinates of the keypoint.
+ */
+ public Point pt;
+ /**
+ * Diameter of the useful keypoint adjacent area.
+ */
+ public float size;
+ /**
+ * Computed orientation of the keypoint (-1 if not applicable).
+ */
+ public float angle;
+ /**
+ * The response, by which the strongest keypoints have been selected. Can
+ * be used for further sorting or subsampling.
+ */
+ public float response;
+ /**
+ * Octave (pyramid layer), from which the keypoint has been extracted.
+ */
+ public int octave;
+ /**
+ * Object ID, that can be used to cluster keypoints by an object they
+ * belong to.
+ */
+ public int class_id;
+
+ // javadoc:KeyPoint::KeyPoint(x,y,_size,_angle,_response,_octave,_class_id)
+ public KeyPoint(float x, float y, float _size, float _angle, float _response, int _octave, int _class_id)
+ {
+ pt = new Point(x, y);
+ size = _size;
+ angle = _angle;
+ response = _response;
+ octave = _octave;
+ class_id = _class_id;
+ }
+
+ // javadoc: KeyPoint::KeyPoint()
+ public KeyPoint()
+ {
+ this(0, 0, 0, -1, 0, 0, -1);
+ }
+
+ // javadoc: KeyPoint::KeyPoint(x, y, _size, _angle, _response, _octave)
+ public KeyPoint(float x, float y, float _size, float _angle, float _response, int _octave)
+ {
+ this(x, y, _size, _angle, _response, _octave, -1);
+ }
+
+ // javadoc: KeyPoint::KeyPoint(x, y, _size, _angle, _response)
+ public KeyPoint(float x, float y, float _size, float _angle, float _response)
+ {
+ this(x, y, _size, _angle, _response, 0, -1);
+ }
+
+ // javadoc: KeyPoint::KeyPoint(x, y, _size, _angle)
+ public KeyPoint(float x, float y, float _size, float _angle)
+ {
+ this(x, y, _size, _angle, 0, 0, -1);
+ }
+
+ // javadoc: KeyPoint::KeyPoint(x, y, _size)
+ public KeyPoint(float x, float y, float _size)
+ {
+ this(x, y, _size, -1, 0, 0, -1);
+ }
+
+ @Override
+ public String toString() {
+ return "KeyPoint [pt=" + pt + ", size=" + size + ", angle=" + angle
+ + ", response=" + response + ", octave=" + octave
+ + ", class_id=" + class_id + "]";
+ }
+
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/Mat.java b/openCVLibrary330/src/main/java/org/opencv/core/Mat.java
new file mode 100644
index 00000000000..6db255417cc
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/Mat.java
@@ -0,0 +1,1316 @@
+package org.opencv.core;
+
+// C++: class Mat
+//javadoc: Mat
+public class Mat {
+
+ public final long nativeObj;
+
+ public Mat(long addr)
+ {
+ if (addr == 0)
+ throw new java.lang.UnsupportedOperationException("Native object address is NULL");
+ nativeObj = addr;
+ }
+
+ //
+ // C++: Mat::Mat()
+ //
+
+ // javadoc: Mat::Mat()
+ public Mat()
+ {
+
+ nativeObj = n_Mat();
+
+ return;
+ }
+
+ //
+ // C++: Mat::Mat(int rows, int cols, int type)
+ //
+
+ // javadoc: Mat::Mat(rows, cols, type)
+ public Mat(int rows, int cols, int type)
+ {
+
+ nativeObj = n_Mat(rows, cols, type);
+
+ return;
+ }
+
+ //
+ // C++: Mat::Mat(Size size, int type)
+ //
+
+ // javadoc: Mat::Mat(size, type)
+ public Mat(Size size, int type)
+ {
+
+ nativeObj = n_Mat(size.width, size.height, type);
+
+ return;
+ }
+
+ //
+ // C++: Mat::Mat(int rows, int cols, int type, Scalar s)
+ //
+
+ // javadoc: Mat::Mat(rows, cols, type, s)
+ public Mat(int rows, int cols, int type, Scalar s)
+ {
+
+ nativeObj = n_Mat(rows, cols, type, s.val[0], s.val[1], s.val[2], s.val[3]);
+
+ return;
+ }
+
+ //
+ // C++: Mat::Mat(Size size, int type, Scalar s)
+ //
+
+ // javadoc: Mat::Mat(size, type, s)
+ public Mat(Size size, int type, Scalar s)
+ {
+
+ nativeObj = n_Mat(size.width, size.height, type, s.val[0], s.val[1], s.val[2], s.val[3]);
+
+ return;
+ }
+
+ //
+ // C++: Mat::Mat(Mat m, Range rowRange, Range colRange = Range::all())
+ //
+
+ // javadoc: Mat::Mat(m, rowRange, colRange)
+ public Mat(Mat m, Range rowRange, Range colRange)
+ {
+
+ nativeObj = n_Mat(m.nativeObj, rowRange.start, rowRange.end, colRange.start, colRange.end);
+
+ return;
+ }
+
+ // javadoc: Mat::Mat(m, rowRange)
+ public Mat(Mat m, Range rowRange)
+ {
+
+ nativeObj = n_Mat(m.nativeObj, rowRange.start, rowRange.end);
+
+ return;
+ }
+
+ //
+ // C++: Mat::Mat(Mat m, Rect roi)
+ //
+
+ // javadoc: Mat::Mat(m, roi)
+ public Mat(Mat m, Rect roi)
+ {
+
+ nativeObj = n_Mat(m.nativeObj, roi.y, roi.y + roi.height, roi.x, roi.x + roi.width);
+
+ return;
+ }
+
+ //
+ // C++: Mat Mat::adjustROI(int dtop, int dbottom, int dleft, int dright)
+ //
+
+ // javadoc: Mat::adjustROI(dtop, dbottom, dleft, dright)
+ public Mat adjustROI(int dtop, int dbottom, int dleft, int dright)
+ {
+
+ Mat retVal = new Mat(n_adjustROI(nativeObj, dtop, dbottom, dleft, dright));
+
+ return retVal;
+ }
+
+ //
+ // C++: void Mat::assignTo(Mat m, int type = -1)
+ //
+
+ // javadoc: Mat::assignTo(m, type)
+ public void assignTo(Mat m, int type)
+ {
+
+ n_assignTo(nativeObj, m.nativeObj, type);
+
+ return;
+ }
+
+ // javadoc: Mat::assignTo(m)
+ public void assignTo(Mat m)
+ {
+
+ n_assignTo(nativeObj, m.nativeObj);
+
+ return;
+ }
+
+ //
+ // C++: int Mat::channels()
+ //
+
+ // javadoc: Mat::channels()
+ public int channels()
+ {
+
+ int retVal = n_channels(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: int Mat::checkVector(int elemChannels, int depth = -1, bool
+ // requireContinuous = true)
+ //
+
+ // javadoc: Mat::checkVector(elemChannels, depth, requireContinuous)
+ public int checkVector(int elemChannels, int depth, boolean requireContinuous)
+ {
+
+ int retVal = n_checkVector(nativeObj, elemChannels, depth, requireContinuous);
+
+ return retVal;
+ }
+
+ // javadoc: Mat::checkVector(elemChannels, depth)
+ public int checkVector(int elemChannels, int depth)
+ {
+
+ int retVal = n_checkVector(nativeObj, elemChannels, depth);
+
+ return retVal;
+ }
+
+ // javadoc: Mat::checkVector(elemChannels)
+ public int checkVector(int elemChannels)
+ {
+
+ int retVal = n_checkVector(nativeObj, elemChannels);
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::clone()
+ //
+
+ // javadoc: Mat::clone()
+ public Mat clone()
+ {
+
+ Mat retVal = new Mat(n_clone(nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::col(int x)
+ //
+
+ // javadoc: Mat::col(x)
+ public Mat col(int x)
+ {
+
+ Mat retVal = new Mat(n_col(nativeObj, x));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::colRange(int startcol, int endcol)
+ //
+
+ // javadoc: Mat::colRange(startcol, endcol)
+ public Mat colRange(int startcol, int endcol)
+ {
+
+ Mat retVal = new Mat(n_colRange(nativeObj, startcol, endcol));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::colRange(Range r)
+ //
+
+ // javadoc: Mat::colRange(r)
+ public Mat colRange(Range r)
+ {
+
+ Mat retVal = new Mat(n_colRange(nativeObj, r.start, r.end));
+
+ return retVal;
+ }
+
+ //
+ // C++: int Mat::dims()
+ //
+
+ // javadoc: Mat::dims()
+ public int dims()
+ {
+
+ int retVal = n_dims(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: int Mat::cols()
+ //
+
+ // javadoc: Mat::cols()
+ public int cols()
+ {
+
+ int retVal = n_cols(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: void Mat::convertTo(Mat& m, int rtype, double alpha = 1, double beta
+ // = 0)
+ //
+
+ // javadoc: Mat::convertTo(m, rtype, alpha, beta)
+ public void convertTo(Mat m, int rtype, double alpha, double beta)
+ {
+
+ n_convertTo(nativeObj, m.nativeObj, rtype, alpha, beta);
+
+ return;
+ }
+
+ // javadoc: Mat::convertTo(m, rtype, alpha)
+ public void convertTo(Mat m, int rtype, double alpha)
+ {
+
+ n_convertTo(nativeObj, m.nativeObj, rtype, alpha);
+
+ return;
+ }
+
+ // javadoc: Mat::convertTo(m, rtype)
+ public void convertTo(Mat m, int rtype)
+ {
+
+ n_convertTo(nativeObj, m.nativeObj, rtype);
+
+ return;
+ }
+
+ //
+ // C++: void Mat::copyTo(Mat& m)
+ //
+
+ // javadoc: Mat::copyTo(m)
+ public void copyTo(Mat m)
+ {
+
+ n_copyTo(nativeObj, m.nativeObj);
+
+ return;
+ }
+
+ //
+ // C++: void Mat::copyTo(Mat& m, Mat mask)
+ //
+
+ // javadoc: Mat::copyTo(m, mask)
+ public void copyTo(Mat m, Mat mask)
+ {
+
+ n_copyTo(nativeObj, m.nativeObj, mask.nativeObj);
+
+ return;
+ }
+
+ //
+ // C++: void Mat::create(int rows, int cols, int type)
+ //
+
+ // javadoc: Mat::create(rows, cols, type)
+ public void create(int rows, int cols, int type)
+ {
+
+ n_create(nativeObj, rows, cols, type);
+
+ return;
+ }
+
+ //
+ // C++: void Mat::create(Size size, int type)
+ //
+
+ // javadoc: Mat::create(size, type)
+ public void create(Size size, int type)
+ {
+
+ n_create(nativeObj, size.width, size.height, type);
+
+ return;
+ }
+
+ //
+ // C++: Mat Mat::cross(Mat m)
+ //
+
+ // javadoc: Mat::cross(m)
+ public Mat cross(Mat m)
+ {
+
+ Mat retVal = new Mat(n_cross(nativeObj, m.nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: long Mat::dataAddr()
+ //
+
+ // javadoc: Mat::dataAddr()
+ public long dataAddr()
+ {
+
+ long retVal = n_dataAddr(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: int Mat::depth()
+ //
+
+ // javadoc: Mat::depth()
+ public int depth()
+ {
+
+ int retVal = n_depth(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::diag(int d = 0)
+ //
+
+ // javadoc: Mat::diag(d)
+ public Mat diag(int d)
+ {
+
+ Mat retVal = new Mat(n_diag(nativeObj, d));
+
+ return retVal;
+ }
+
+ // javadoc: Mat::diag()
+ public Mat diag()
+ {
+
+ Mat retVal = new Mat(n_diag(nativeObj, 0));
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::diag(Mat d)
+ //
+
+ // javadoc: Mat::diag(d)
+ public static Mat diag(Mat d)
+ {
+
+ Mat retVal = new Mat(n_diag(d.nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: double Mat::dot(Mat m)
+ //
+
+ // javadoc: Mat::dot(m)
+ public double dot(Mat m)
+ {
+
+ double retVal = n_dot(nativeObj, m.nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: size_t Mat::elemSize()
+ //
+
+ // javadoc: Mat::elemSize()
+ public long elemSize()
+ {
+
+ long retVal = n_elemSize(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: size_t Mat::elemSize1()
+ //
+
+ // javadoc: Mat::elemSize1()
+ public long elemSize1()
+ {
+
+ long retVal = n_elemSize1(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: bool Mat::empty()
+ //
+
+ // javadoc: Mat::empty()
+ public boolean empty()
+ {
+
+ boolean retVal = n_empty(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::eye(int rows, int cols, int type)
+ //
+
+ // javadoc: Mat::eye(rows, cols, type)
+ public static Mat eye(int rows, int cols, int type)
+ {
+
+ Mat retVal = new Mat(n_eye(rows, cols, type));
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::eye(Size size, int type)
+ //
+
+ // javadoc: Mat::eye(size, type)
+ public static Mat eye(Size size, int type)
+ {
+
+ Mat retVal = new Mat(n_eye(size.width, size.height, type));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::inv(int method = DECOMP_LU)
+ //
+
+ // javadoc: Mat::inv(method)
+ public Mat inv(int method)
+ {
+
+ Mat retVal = new Mat(n_inv(nativeObj, method));
+
+ return retVal;
+ }
+
+ // javadoc: Mat::inv()
+ public Mat inv()
+ {
+
+ Mat retVal = new Mat(n_inv(nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: bool Mat::isContinuous()
+ //
+
+ // javadoc: Mat::isContinuous()
+ public boolean isContinuous()
+ {
+
+ boolean retVal = n_isContinuous(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: bool Mat::isSubmatrix()
+ //
+
+ // javadoc: Mat::isSubmatrix()
+ public boolean isSubmatrix()
+ {
+
+ boolean retVal = n_isSubmatrix(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: void Mat::locateROI(Size wholeSize, Point ofs)
+ //
+
+ // javadoc: Mat::locateROI(wholeSize, ofs)
+ public void locateROI(Size wholeSize, Point ofs)
+ {
+ double[] wholeSize_out = new double[2];
+ double[] ofs_out = new double[2];
+ locateROI_0(nativeObj, wholeSize_out, ofs_out);
+ if(wholeSize!=null){ wholeSize.width = wholeSize_out[0]; wholeSize.height = wholeSize_out[1]; }
+ if(ofs!=null){ ofs.x = ofs_out[0]; ofs.y = ofs_out[1]; }
+ return;
+ }
+
+ //
+ // C++: Mat Mat::mul(Mat m, double scale = 1)
+ //
+
+ // javadoc: Mat::mul(m, scale)
+ public Mat mul(Mat m, double scale)
+ {
+
+ Mat retVal = new Mat(n_mul(nativeObj, m.nativeObj, scale));
+
+ return retVal;
+ }
+
+ // javadoc: Mat::mul(m)
+ public Mat mul(Mat m)
+ {
+
+ Mat retVal = new Mat(n_mul(nativeObj, m.nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::ones(int rows, int cols, int type)
+ //
+
+ // javadoc: Mat::ones(rows, cols, type)
+ public static Mat ones(int rows, int cols, int type)
+ {
+
+ Mat retVal = new Mat(n_ones(rows, cols, type));
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::ones(Size size, int type)
+ //
+
+ // javadoc: Mat::ones(size, type)
+ public static Mat ones(Size size, int type)
+ {
+
+ Mat retVal = new Mat(n_ones(size.width, size.height, type));
+
+ return retVal;
+ }
+
+ //
+ // C++: void Mat::push_back(Mat m)
+ //
+
+ // javadoc: Mat::push_back(m)
+ public void push_back(Mat m)
+ {
+
+ n_push_back(nativeObj, m.nativeObj);
+
+ return;
+ }
+
+ //
+ // C++: void Mat::release()
+ //
+
+ // javadoc: Mat::release()
+ public void release()
+ {
+
+ n_release(nativeObj);
+
+ return;
+ }
+
+ //
+ // C++: Mat Mat::reshape(int cn, int rows = 0)
+ //
+
+ // javadoc: Mat::reshape(cn, rows)
+ public Mat reshape(int cn, int rows)
+ {
+
+ Mat retVal = new Mat(n_reshape(nativeObj, cn, rows));
+
+ return retVal;
+ }
+
+ // javadoc: Mat::reshape(cn)
+ public Mat reshape(int cn)
+ {
+
+ Mat retVal = new Mat(n_reshape(nativeObj, cn));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::row(int y)
+ //
+
+ // javadoc: Mat::row(y)
+ public Mat row(int y)
+ {
+
+ Mat retVal = new Mat(n_row(nativeObj, y));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::rowRange(int startrow, int endrow)
+ //
+
+ // javadoc: Mat::rowRange(startrow, endrow)
+ public Mat rowRange(int startrow, int endrow)
+ {
+
+ Mat retVal = new Mat(n_rowRange(nativeObj, startrow, endrow));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::rowRange(Range r)
+ //
+
+ // javadoc: Mat::rowRange(r)
+ public Mat rowRange(Range r)
+ {
+
+ Mat retVal = new Mat(n_rowRange(nativeObj, r.start, r.end));
+
+ return retVal;
+ }
+
+ //
+ // C++: int Mat::rows()
+ //
+
+ // javadoc: Mat::rows()
+ public int rows()
+ {
+
+ int retVal = n_rows(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::operator =(Scalar s)
+ //
+
+ // javadoc: Mat::operator =(s)
+ public Mat setTo(Scalar s)
+ {
+
+ Mat retVal = new Mat(n_setTo(nativeObj, s.val[0], s.val[1], s.val[2], s.val[3]));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::setTo(Scalar value, Mat mask = Mat())
+ //
+
+ // javadoc: Mat::setTo(value, mask)
+ public Mat setTo(Scalar value, Mat mask)
+ {
+
+ Mat retVal = new Mat(n_setTo(nativeObj, value.val[0], value.val[1], value.val[2], value.val[3], mask.nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::setTo(Mat value, Mat mask = Mat())
+ //
+
+ // javadoc: Mat::setTo(value, mask)
+ public Mat setTo(Mat value, Mat mask)
+ {
+
+ Mat retVal = new Mat(n_setTo(nativeObj, value.nativeObj, mask.nativeObj));
+
+ return retVal;
+ }
+
+ // javadoc: Mat::setTo(value)
+ public Mat setTo(Mat value)
+ {
+
+ Mat retVal = new Mat(n_setTo(nativeObj, value.nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: Size Mat::size()
+ //
+
+ // javadoc: Mat::size()
+ public Size size()
+ {
+
+ Size retVal = new Size(n_size(nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: size_t Mat::step1(int i = 0)
+ //
+
+ // javadoc: Mat::step1(i)
+ public long step1(int i)
+ {
+
+ long retVal = n_step1(nativeObj, i);
+
+ return retVal;
+ }
+
+ // javadoc: Mat::step1()
+ public long step1()
+ {
+
+ long retVal = n_step1(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::operator()(int rowStart, int rowEnd, int colStart, int
+ // colEnd)
+ //
+
+ // javadoc: Mat::operator()(rowStart, rowEnd, colStart, colEnd)
+ public Mat submat(int rowStart, int rowEnd, int colStart, int colEnd)
+ {
+
+ Mat retVal = new Mat(n_submat_rr(nativeObj, rowStart, rowEnd, colStart, colEnd));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::operator()(Range rowRange, Range colRange)
+ //
+
+ // javadoc: Mat::operator()(rowRange, colRange)
+ public Mat submat(Range rowRange, Range colRange)
+ {
+
+ Mat retVal = new Mat(n_submat_rr(nativeObj, rowRange.start, rowRange.end, colRange.start, colRange.end));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::operator()(Rect roi)
+ //
+
+ // javadoc: Mat::operator()(roi)
+ public Mat submat(Rect roi)
+ {
+
+ Mat retVal = new Mat(n_submat(nativeObj, roi.x, roi.y, roi.width, roi.height));
+
+ return retVal;
+ }
+
+ //
+ // C++: Mat Mat::t()
+ //
+
+ // javadoc: Mat::t()
+ public Mat t()
+ {
+
+ Mat retVal = new Mat(n_t(nativeObj));
+
+ return retVal;
+ }
+
+ //
+ // C++: size_t Mat::total()
+ //
+
+ // javadoc: Mat::total()
+ public long total()
+ {
+
+ long retVal = n_total(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: int Mat::type()
+ //
+
+ // javadoc: Mat::type()
+ public int type()
+ {
+
+ int retVal = n_type(nativeObj);
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::zeros(int rows, int cols, int type)
+ //
+
+ // javadoc: Mat::zeros(rows, cols, type)
+ public static Mat zeros(int rows, int cols, int type)
+ {
+
+ Mat retVal = new Mat(n_zeros(rows, cols, type));
+
+ return retVal;
+ }
+
+ //
+ // C++: static Mat Mat::zeros(Size size, int type)
+ //
+
+ // javadoc: Mat::zeros(size, type)
+ public static Mat zeros(Size size, int type)
+ {
+
+ Mat retVal = new Mat(n_zeros(size.width, size.height, type));
+
+ return retVal;
+ }
+
+ @Override
+ protected void finalize() throws Throwable {
+ n_delete(nativeObj);
+ super.finalize();
+ }
+
+ // javadoc:Mat::toString()
+ @Override
+ public String toString() {
+ return "Mat [ " +
+ rows() + "*" + cols() + "*" + CvType.typeToString(type()) +
+ ", isCont=" + isContinuous() + ", isSubmat=" + isSubmatrix() +
+ ", nativeObj=0x" + Long.toHexString(nativeObj) +
+ ", dataAddr=0x" + Long.toHexString(dataAddr()) +
+ " ]";
+ }
+
+ // javadoc:Mat::dump()
+ public String dump() {
+ return nDump(nativeObj);
+ }
+
+ // javadoc:Mat::put(row,col,data)
+ public int put(int row, int col, double... data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ return nPutD(nativeObj, row, col, data.length, data);
+ }
+
+ // javadoc:Mat::put(row,col,data)
+ public int put(int row, int col, float[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_32F) {
+ return nPutF(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::put(row,col,data)
+ public int put(int row, int col, int[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_32S) {
+ return nPutI(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::put(row,col,data)
+ public int put(int row, int col, short[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_16U || CvType.depth(t) == CvType.CV_16S) {
+ return nPutS(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::put(row,col,data)
+ public int put(int row, int col, byte[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_8U || CvType.depth(t) == CvType.CV_8S) {
+ return nPutB(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::get(row,col,data)
+ public int get(int row, int col, byte[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_8U || CvType.depth(t) == CvType.CV_8S) {
+ return nGetB(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::get(row,col,data)
+ public int get(int row, int col, short[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_16U || CvType.depth(t) == CvType.CV_16S) {
+ return nGetS(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::get(row,col,data)
+ public int get(int row, int col, int[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_32S) {
+ return nGetI(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::get(row,col,data)
+ public int get(int row, int col, float[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_32F) {
+ return nGetF(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::get(row,col,data)
+ public int get(int row, int col, double[] data) {
+ int t = type();
+ if (data == null || data.length % CvType.channels(t) != 0)
+ throw new java.lang.UnsupportedOperationException(
+ "Provided data element number (" +
+ (data == null ? 0 : data.length) +
+ ") should be multiple of the Mat channels count (" +
+ CvType.channels(t) + ")");
+ if (CvType.depth(t) == CvType.CV_64F) {
+ return nGetD(nativeObj, row, col, data.length, data);
+ }
+ throw new java.lang.UnsupportedOperationException("Mat data type is not compatible: " + t);
+ }
+
+ // javadoc:Mat::get(row,col)
+ public double[] get(int row, int col) {
+ return nGet(nativeObj, row, col);
+ }
+
+ // javadoc:Mat::height()
+ public int height() {
+ return rows();
+ }
+
+ // javadoc:Mat::width()
+ public int width() {
+ return cols();
+ }
+
+ // javadoc:Mat::getNativeObjAddr()
+ public long getNativeObjAddr() {
+ return nativeObj;
+ }
+
+ // C++: Mat::Mat()
+ private static native long n_Mat();
+
+ // C++: Mat::Mat(int rows, int cols, int type)
+ private static native long n_Mat(int rows, int cols, int type);
+
+ // C++: Mat::Mat(Size size, int type)
+ private static native long n_Mat(double size_width, double size_height, int type);
+
+ // C++: Mat::Mat(int rows, int cols, int type, Scalar s)
+ private static native long n_Mat(int rows, int cols, int type, double s_val0, double s_val1, double s_val2, double s_val3);
+
+ // C++: Mat::Mat(Size size, int type, Scalar s)
+ private static native long n_Mat(double size_width, double size_height, int type, double s_val0, double s_val1, double s_val2, double s_val3);
+
+ // C++: Mat::Mat(Mat m, Range rowRange, Range colRange = Range::all())
+ private static native long n_Mat(long m_nativeObj, int rowRange_start, int rowRange_end, int colRange_start, int colRange_end);
+
+ private static native long n_Mat(long m_nativeObj, int rowRange_start, int rowRange_end);
+
+ // C++: Mat Mat::adjustROI(int dtop, int dbottom, int dleft, int dright)
+ private static native long n_adjustROI(long nativeObj, int dtop, int dbottom, int dleft, int dright);
+
+ // C++: void Mat::assignTo(Mat m, int type = -1)
+ private static native void n_assignTo(long nativeObj, long m_nativeObj, int type);
+
+ private static native void n_assignTo(long nativeObj, long m_nativeObj);
+
+ // C++: int Mat::channels()
+ private static native int n_channels(long nativeObj);
+
+ // C++: int Mat::checkVector(int elemChannels, int depth = -1, bool
+ // requireContinuous = true)
+ private static native int n_checkVector(long nativeObj, int elemChannels, int depth, boolean requireContinuous);
+
+ private static native int n_checkVector(long nativeObj, int elemChannels, int depth);
+
+ private static native int n_checkVector(long nativeObj, int elemChannels);
+
+ // C++: Mat Mat::clone()
+ private static native long n_clone(long nativeObj);
+
+ // C++: Mat Mat::col(int x)
+ private static native long n_col(long nativeObj, int x);
+
+ // C++: Mat Mat::colRange(int startcol, int endcol)
+ private static native long n_colRange(long nativeObj, int startcol, int endcol);
+
+ // C++: int Mat::dims()
+ private static native int n_dims(long nativeObj);
+
+ // C++: int Mat::cols()
+ private static native int n_cols(long nativeObj);
+
+ // C++: void Mat::convertTo(Mat& m, int rtype, double alpha = 1, double beta
+ // = 0)
+ private static native void n_convertTo(long nativeObj, long m_nativeObj, int rtype, double alpha, double beta);
+
+ private static native void n_convertTo(long nativeObj, long m_nativeObj, int rtype, double alpha);
+
+ private static native void n_convertTo(long nativeObj, long m_nativeObj, int rtype);
+
+ // C++: void Mat::copyTo(Mat& m)
+ private static native void n_copyTo(long nativeObj, long m_nativeObj);
+
+ // C++: void Mat::copyTo(Mat& m, Mat mask)
+ private static native void n_copyTo(long nativeObj, long m_nativeObj, long mask_nativeObj);
+
+ // C++: void Mat::create(int rows, int cols, int type)
+ private static native void n_create(long nativeObj, int rows, int cols, int type);
+
+ // C++: void Mat::create(Size size, int type)
+ private static native void n_create(long nativeObj, double size_width, double size_height, int type);
+
+ // C++: Mat Mat::cross(Mat m)
+ private static native long n_cross(long nativeObj, long m_nativeObj);
+
+ // C++: long Mat::dataAddr()
+ private static native long n_dataAddr(long nativeObj);
+
+ // C++: int Mat::depth()
+ private static native int n_depth(long nativeObj);
+
+ // C++: Mat Mat::diag(int d = 0)
+ private static native long n_diag(long nativeObj, int d);
+
+ // C++: static Mat Mat::diag(Mat d)
+ private static native long n_diag(long d_nativeObj);
+
+ // C++: double Mat::dot(Mat m)
+ private static native double n_dot(long nativeObj, long m_nativeObj);
+
+ // C++: size_t Mat::elemSize()
+ private static native long n_elemSize(long nativeObj);
+
+ // C++: size_t Mat::elemSize1()
+ private static native long n_elemSize1(long nativeObj);
+
+ // C++: bool Mat::empty()
+ private static native boolean n_empty(long nativeObj);
+
+ // C++: static Mat Mat::eye(int rows, int cols, int type)
+ private static native long n_eye(int rows, int cols, int type);
+
+ // C++: static Mat Mat::eye(Size size, int type)
+ private static native long n_eye(double size_width, double size_height, int type);
+
+ // C++: Mat Mat::inv(int method = DECOMP_LU)
+ private static native long n_inv(long nativeObj, int method);
+
+ private static native long n_inv(long nativeObj);
+
+ // C++: bool Mat::isContinuous()
+ private static native boolean n_isContinuous(long nativeObj);
+
+ // C++: bool Mat::isSubmatrix()
+ private static native boolean n_isSubmatrix(long nativeObj);
+
+ // C++: void Mat::locateROI(Size wholeSize, Point ofs)
+ private static native void locateROI_0(long nativeObj, double[] wholeSize_out, double[] ofs_out);
+
+ // C++: Mat Mat::mul(Mat m, double scale = 1)
+ private static native long n_mul(long nativeObj, long m_nativeObj, double scale);
+
+ private static native long n_mul(long nativeObj, long m_nativeObj);
+
+ // C++: static Mat Mat::ones(int rows, int cols, int type)
+ private static native long n_ones(int rows, int cols, int type);
+
+ // C++: static Mat Mat::ones(Size size, int type)
+ private static native long n_ones(double size_width, double size_height, int type);
+
+ // C++: void Mat::push_back(Mat m)
+ private static native void n_push_back(long nativeObj, long m_nativeObj);
+
+ // C++: void Mat::release()
+ private static native void n_release(long nativeObj);
+
+ // C++: Mat Mat::reshape(int cn, int rows = 0)
+ private static native long n_reshape(long nativeObj, int cn, int rows);
+
+ private static native long n_reshape(long nativeObj, int cn);
+
+ // C++: Mat Mat::row(int y)
+ private static native long n_row(long nativeObj, int y);
+
+ // C++: Mat Mat::rowRange(int startrow, int endrow)
+ private static native long n_rowRange(long nativeObj, int startrow, int endrow);
+
+ // C++: int Mat::rows()
+ private static native int n_rows(long nativeObj);
+
+ // C++: Mat Mat::operator =(Scalar s)
+ private static native long n_setTo(long nativeObj, double s_val0, double s_val1, double s_val2, double s_val3);
+
+ // C++: Mat Mat::setTo(Scalar value, Mat mask = Mat())
+ private static native long n_setTo(long nativeObj, double s_val0, double s_val1, double s_val2, double s_val3, long mask_nativeObj);
+
+ // C++: Mat Mat::setTo(Mat value, Mat mask = Mat())
+ private static native long n_setTo(long nativeObj, long value_nativeObj, long mask_nativeObj);
+
+ private static native long n_setTo(long nativeObj, long value_nativeObj);
+
+ // C++: Size Mat::size()
+ private static native double[] n_size(long nativeObj);
+
+ // C++: size_t Mat::step1(int i = 0)
+ private static native long n_step1(long nativeObj, int i);
+
+ private static native long n_step1(long nativeObj);
+
+ // C++: Mat Mat::operator()(Range rowRange, Range colRange)
+ private static native long n_submat_rr(long nativeObj, int rowRange_start, int rowRange_end, int colRange_start, int colRange_end);
+
+ // C++: Mat Mat::operator()(Rect roi)
+ private static native long n_submat(long nativeObj, int roi_x, int roi_y, int roi_width, int roi_height);
+
+ // C++: Mat Mat::t()
+ private static native long n_t(long nativeObj);
+
+ // C++: size_t Mat::total()
+ private static native long n_total(long nativeObj);
+
+ // C++: int Mat::type()
+ private static native int n_type(long nativeObj);
+
+ // C++: static Mat Mat::zeros(int rows, int cols, int type)
+ private static native long n_zeros(int rows, int cols, int type);
+
+ // C++: static Mat Mat::zeros(Size size, int type)
+ private static native long n_zeros(double size_width, double size_height, int type);
+
+ // native support for java finalize()
+ private static native void n_delete(long nativeObj);
+
+ private static native int nPutD(long self, int row, int col, int count, double[] data);
+
+ private static native int nPutF(long self, int row, int col, int count, float[] data);
+
+ private static native int nPutI(long self, int row, int col, int count, int[] data);
+
+ private static native int nPutS(long self, int row, int col, int count, short[] data);
+
+ private static native int nPutB(long self, int row, int col, int count, byte[] data);
+
+ private static native int nGetB(long self, int row, int col, int count, byte[] vals);
+
+ private static native int nGetS(long self, int row, int col, int count, short[] vals);
+
+ private static native int nGetI(long self, int row, int col, int count, int[] vals);
+
+ private static native int nGetF(long self, int row, int col, int count, float[] vals);
+
+ private static native int nGetD(long self, int row, int col, int count, double[] vals);
+
+ private static native double[] nGet(long self, int row, int col);
+
+ private static native String nDump(long self);
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfByte.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfByte.java
new file mode 100644
index 00000000000..7756eb94f95
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfByte.java
@@ -0,0 +1,79 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class MatOfByte extends Mat {
+ // 8UC(x)
+ private static final int _depth = CvType.CV_8U;
+ private static final int _channels = 1;
+
+ public MatOfByte() {
+ super();
+ }
+
+ protected MatOfByte(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfByte fromNativeAddr(long addr) {
+ return new MatOfByte(addr);
+ }
+
+ public MatOfByte(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfByte(byte...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(byte...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public byte[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ byte[] a = new byte[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Byte ab[] = lb.toArray(new Byte[0]);
+ byte a[] = new byte[ab.length];
+ for(int i=0; i toList() {
+ byte[] a = toArray();
+ Byte ab[] = new Byte[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+
+ public void fromArray(DMatch...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ float buff[] = new float[num * _channels];
+ for(int i=0; i ldm) {
+ DMatch adm[] = ldm.toArray(new DMatch[0]);
+ fromArray(adm);
+ }
+
+ public List toList() {
+ DMatch[] adm = toArray();
+ return Arrays.asList(adm);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfDouble.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfDouble.java
new file mode 100644
index 00000000000..1a8e23ca30f
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfDouble.java
@@ -0,0 +1,79 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class MatOfDouble extends Mat {
+ // 64FC(x)
+ private static final int _depth = CvType.CV_64F;
+ private static final int _channels = 1;
+
+ public MatOfDouble() {
+ super();
+ }
+
+ protected MatOfDouble(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfDouble fromNativeAddr(long addr) {
+ return new MatOfDouble(addr);
+ }
+
+ public MatOfDouble(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfDouble(double...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(double...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public double[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ double[] a = new double[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Double ab[] = lb.toArray(new Double[0]);
+ double a[] = new double[ab.length];
+ for(int i=0; i toList() {
+ double[] a = toArray();
+ Double ab[] = new Double[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(float...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public float[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ float[] a = new float[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Float ab[] = lb.toArray(new Float[0]);
+ float a[] = new float[ab.length];
+ for(int i=0; i toList() {
+ float[] a = toArray();
+ Float ab[] = new Float[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(float...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public float[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ float[] a = new float[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Float ab[] = lb.toArray(new Float[0]);
+ float a[] = new float[ab.length];
+ for(int i=0; i toList() {
+ float[] a = toArray();
+ Float ab[] = new Float[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(float...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public float[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ float[] a = new float[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Float ab[] = lb.toArray(new Float[0]);
+ float a[] = new float[ab.length];
+ for(int i=0; i toList() {
+ float[] a = toArray();
+ Float ab[] = new Float[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(int...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public int[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ int[] a = new int[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Integer ab[] = lb.toArray(new Integer[0]);
+ int a[] = new int[ab.length];
+ for(int i=0; i toList() {
+ int[] a = toArray();
+ Integer ab[] = new Integer[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(int...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length / _channels;
+ alloc(num);
+ put(0, 0, a); //TODO: check ret val!
+ }
+
+ public int[] toArray() {
+ int num = checkVector(_channels, _depth);
+ if(num < 0)
+ throw new RuntimeException("Native Mat has unexpected type or size: " + toString());
+ int[] a = new int[num * _channels];
+ if(num == 0)
+ return a;
+ get(0, 0, a); //TODO: check ret val!
+ return a;
+ }
+
+ public void fromList(List lb) {
+ if(lb==null || lb.size()==0)
+ return;
+ Integer ab[] = lb.toArray(new Integer[0]);
+ int a[] = new int[ab.length];
+ for(int i=0; i toList() {
+ int[] a = toArray();
+ Integer ab[] = new Integer[a.length];
+ for(int i=0; i0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(KeyPoint...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ float buff[] = new float[num * _channels];
+ for(int i=0; i lkp) {
+ KeyPoint akp[] = lkp.toArray(new KeyPoint[0]);
+ fromArray(akp);
+ }
+
+ public List toList() {
+ KeyPoint[] akp = toArray();
+ return Arrays.asList(akp);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint.java
new file mode 100644
index 00000000000..f4d573bb734
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint.java
@@ -0,0 +1,78 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class MatOfPoint extends Mat {
+ // 32SC2
+ private static final int _depth = CvType.CV_32S;
+ private static final int _channels = 2;
+
+ public MatOfPoint() {
+ super();
+ }
+
+ protected MatOfPoint(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfPoint fromNativeAddr(long addr) {
+ return new MatOfPoint(addr);
+ }
+
+ public MatOfPoint(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfPoint(Point...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(Point...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ int buff[] = new int[num * _channels];
+ for(int i=0; i lp) {
+ Point ap[] = lp.toArray(new Point[0]);
+ fromArray(ap);
+ }
+
+ public List toList() {
+ Point[] ap = toArray();
+ return Arrays.asList(ap);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint2f.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint2f.java
new file mode 100644
index 00000000000..4b8c926fffa
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint2f.java
@@ -0,0 +1,78 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class MatOfPoint2f extends Mat {
+ // 32FC2
+ private static final int _depth = CvType.CV_32F;
+ private static final int _channels = 2;
+
+ public MatOfPoint2f() {
+ super();
+ }
+
+ protected MatOfPoint2f(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfPoint2f fromNativeAddr(long addr) {
+ return new MatOfPoint2f(addr);
+ }
+
+ public MatOfPoint2f(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfPoint2f(Point...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(Point...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ float buff[] = new float[num * _channels];
+ for(int i=0; i lp) {
+ Point ap[] = lp.toArray(new Point[0]);
+ fromArray(ap);
+ }
+
+ public List toList() {
+ Point[] ap = toArray();
+ return Arrays.asList(ap);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint3.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint3.java
new file mode 100644
index 00000000000..3b50561e914
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint3.java
@@ -0,0 +1,79 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class MatOfPoint3 extends Mat {
+ // 32SC3
+ private static final int _depth = CvType.CV_32S;
+ private static final int _channels = 3;
+
+ public MatOfPoint3() {
+ super();
+ }
+
+ protected MatOfPoint3(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfPoint3 fromNativeAddr(long addr) {
+ return new MatOfPoint3(addr);
+ }
+
+ public MatOfPoint3(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfPoint3(Point3...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(Point3...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ int buff[] = new int[num * _channels];
+ for(int i=0; i lp) {
+ Point3 ap[] = lp.toArray(new Point3[0]);
+ fromArray(ap);
+ }
+
+ public List toList() {
+ Point3[] ap = toArray();
+ return Arrays.asList(ap);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint3f.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint3f.java
new file mode 100644
index 00000000000..fc5fee436d7
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfPoint3f.java
@@ -0,0 +1,79 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class MatOfPoint3f extends Mat {
+ // 32FC3
+ private static final int _depth = CvType.CV_32F;
+ private static final int _channels = 3;
+
+ public MatOfPoint3f() {
+ super();
+ }
+
+ protected MatOfPoint3f(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfPoint3f fromNativeAddr(long addr) {
+ return new MatOfPoint3f(addr);
+ }
+
+ public MatOfPoint3f(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfPoint3f(Point3...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(Point3...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ float buff[] = new float[num * _channels];
+ for(int i=0; i lp) {
+ Point3 ap[] = lp.toArray(new Point3[0]);
+ fromArray(ap);
+ }
+
+ public List toList() {
+ Point3[] ap = toArray();
+ return Arrays.asList(ap);
+ }
+}
diff --git a/openCVLibrary330/src/main/java/org/opencv/core/MatOfRect.java b/openCVLibrary330/src/main/java/org/opencv/core/MatOfRect.java
new file mode 100644
index 00000000000..ec0fb01e4ac
--- /dev/null
+++ b/openCVLibrary330/src/main/java/org/opencv/core/MatOfRect.java
@@ -0,0 +1,81 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+
+public class MatOfRect extends Mat {
+ // 32SC4
+ private static final int _depth = CvType.CV_32S;
+ private static final int _channels = 4;
+
+ public MatOfRect() {
+ super();
+ }
+
+ protected MatOfRect(long addr) {
+ super(addr);
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public static MatOfRect fromNativeAddr(long addr) {
+ return new MatOfRect(addr);
+ }
+
+ public MatOfRect(Mat m) {
+ super(m, Range.all());
+ if( !empty() && checkVector(_channels, _depth) < 0 )
+ throw new IllegalArgumentException("Incompatible Mat");
+ //FIXME: do we need release() here?
+ }
+
+ public MatOfRect(Rect...a) {
+ super();
+ fromArray(a);
+ }
+
+ public void alloc(int elemNumber) {
+ if(elemNumber>0)
+ super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+ }
+
+ public void fromArray(Rect...a) {
+ if(a==null || a.length==0)
+ return;
+ int num = a.length;
+ alloc(num);
+ int buff[] = new int[num * _channels];
+ for(int i=0; i lr) {
+ Rect ap[] = lr.toArray(new Rect[0]);
+ fromArray(ap);
+ }
+
+ public List