Java VectorF类代码示例

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
public VectorF getVuMarkPosition()
{
    VectorF targetPos = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetPos = pose.getTranslation();
            robot.tracer.traceInfo("TargetPos", "%s: x=%6.2f, y=%6.2f, z=%6.2f",
                                   vuMark.toString(),
                                   targetPos.get(0)/RobotInfo.MM_PER_INCH,
                                   targetPos.get(1)/RobotInfo.MM_PER_INCH,
                                   -targetPos.get(2)/RobotInfo.MM_PER_INCH);
        }
    }

    return targetPos;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
public void updateLocation(){
    OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();
    telemetry.addData("Pose", format(pose));

    /* We further illustrate how to decompose the pose into useful rotational and
    * translational components */
    if (pose != null) {
        VectorF trans = pose.getTranslation();
        Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);

        // Extract the X, Y, and Z components of the offset of the target relative to the robot
        tX = trans.get(0);
        tY = trans.get(1);
        tZ = trans.get(2);

        // Extract the rotational components of the target relative to the robot
        rX = rot.firstAngle;
        rY = rot.secondAngle;
        rZ = rot.thirdAngle;
        //Z is forward-backward
        //x is sideways
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
private VectorF getVuMarkPosition()
{
    VectorF targetPos = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetPos = pose.getTranslation();
        }
    }

    return targetPos;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
public VectorF navOffWall(VectorF trans, double robotAngle, VectorF offWall){
    return new VectorF((float) (trans.get(0) - offWall.get(0) *
            Math.sin(Math.toRadians(robotAngle)) - offWall.get(2) *
            Math.cos(Math.toRadians(robotAngle))), trans.get(1),
            (float) (trans.get(2) + offWall.get(0) *
                    Math.cos(Math.toRadians(robotAngle)) - offWall.get(2) *
                    Math.sin(Math.toRadians(robotAngle))));
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
public VectorF anglesFromTarget(VuforiaTrackableDefaultListener image){
    float [] data = image.getRawPose().getData();
    float [] [] rotation = {{data[0], data[1]}, {data[4], data[5], data[6]}, {data[8], data[9], data[10]}};
    double thetaX = Math.atan2(rotation[2][1], rotation[2][2]);
    double thetaY = Math.atan2(-rotation[2][0], Math.sqrt(rotation[2][1] * rotation[2][1] + rotation[2][2] * rotation[2][2]));
    double thetaZ = Math.atan2(rotation[1][0], rotation[0][0]);
    return new VectorF((float)thetaX, (float)thetaY, (float)thetaZ);
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
@Override
    public void runPeriodic(double elapsedTime)
    {
        double left = gamepad.getLeftStickY(true);
        double right = gamepad.getRightStickY(true);
        driveBase.tankDrive(left, right);

        final int LABEL_WIDTH = 180;
        dashboard.displayPrintf(1, LABEL_WIDTH, "Power(L/R) = ", "%.2f/%.2f", left, right);
        dashboard.displayPrintf(2, LABEL_WIDTH, "GyroHeading = ", "%.2f", gyro.getZHeading().value);
        dashboard.displayPrintf(3, LABEL_WIDTH, "SoundEnvelope = ", "%s", envelopeToggle.getState()? "ON": "OFF");
        dashboard.displayPrintf(4, LABEL_WIDTH, "ToneDevice = ", "%s",
                                analogToneToggle.getState()? "AnalogOut": "Android");
        dashboard.displayPrintf(5, LABEL_WIDTH, "Vision = ", "%s (%d)", visionEnabled, targetIndex);

        if (isVisionEnabled())
        {
            boolean followTarget = followTargetToggle.getState();

            for (int i = 0; i < targets.length; i++)
            {
                VuforiaTrackable target = vuforia.getTarget(i);
                boolean visible = vuforia.isTargetVisible(target);

                if (USE_SPEECH)
                {
                    if (visible != targetsFound[i])
                    {
                        targetsFound[i] = visible;
                        String sentence = String.format(
                                "%s is %s.", target.getName(), visible ? "in view" : "out of view");
                        textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
                    }
                }

                if (visible)
                {
                    String label = String.format(i == targetIndex ? "<%s> = " : "%s = ", target.getName());
                    OpenGLMatrix pose = vuforia.getTargetPose(target);
                    if (pose != null)
                    {
                        VectorF translation = pose.getTranslation();
                        float targetX = translation.get(0) / MM_PER_INCH;
                        float targetY = translation.get(1) / MM_PER_INCH;
                        float targetZ = -translation.get(2) / MM_PER_INCH;

                        dashboard.displayPrintf(
                                i + 5, LABEL_WIDTH, label, "%6.2f,%6.2f,%6.2f", targetX, targetY, targetZ);

                        if (followTarget && i == targetIndex)
                        {
//                            targetDistance = targetZ;
                            targetDistance = 24.0;
                            // TODO: doesn't work! Event won't fire until distance is satisfied.
                            if (visionEvent.isSignaled())
                            {
                                targetHeading = Math.toDegrees(Math.atan2(targetX, targetZ));
                                visionEvent.set(false);
                                visionPidDrive.setTarget(24.0, targetHeading, false, visionEvent);
                            }
                        }
                    }

                    OpenGLMatrix robotLocation = vuforia.getRobotLocation(target);
                    if (robotLocation != null)
                    {
                        lastKnownRobotLocation = robotLocation;
                    }
                }
            }

            if (lastKnownRobotLocation != null)
            {
                dashboard.displayPrintf(9, LABEL_WIDTH, "RobotLoc = ", lastKnownRobotLocation.formatAsTransform());
            }
        }
    }
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
@Override
public void runPeriodic(double elapsedTime)
{
    double startTime;

    startTime = TrcUtil.getCurrentTime();
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(vuforia.getTarget(0));
    dashboard.displayPrintf(1, "ElapseTime: getTarget=%f", TrcUtil.getCurrentTime() - startTime);

    if (vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        startTime = TrcUtil.getCurrentTime();
        VectorF pos = getVuMarkPosition();
        Orientation orientation = getVuMarkOrientation();
        dashboard.displayPrintf(2, "ElapseTime: getVuMarkInfo=%f", TrcUtil.getCurrentTime() - startTime);

        dashboard.displayPrintf(3, "%s: x=%6.2f,y=%6.2f,z=%6.2f",
                vuMark.toString(), pos.get(0)/MM_PER_INCH, pos.get(1)/MM_PER_INCH, pos.get(2)/MM_PER_INCH);
        dashboard.displayPrintf(4, "%s: xRot=%6.2f,yRot=%6.2f,zRot=%6.2f",
                vuMark.toString(), orientation.firstAngle, orientation.secondAngle, orientation.thirdAngle);
    }

    if (vuMark != prevVuMark)
    {
        String sentence = null;

        if (vuMark != RelicRecoveryVuMark.UNKNOWN)
        {
            sentence = String.format("%s is %s.", vuMark.toString(), "in view");
        }
        else if (prevVuMark != null)
        {
            sentence = String.format("%s is %s.", prevVuMark.toString(), "out of view");
        }

        if (sentence != null)
        {
            dashboard.displayPrintf(5, sentence);
            if (textToSpeech != null)
            {
                //
                // ZTE phones are on KitKat and running level 19 APIs, so we need to use the deprecated version
                // to be compatible with it.
                //
                textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
            }
        }

        prevVuMark = vuMark;
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
@Override
public void runPeriodic(double elapsedTime)
{
    final int LABEL_WIDTH = 120;

    for (int i = 0; i < targets.length; i++)
    {
        VuforiaTrackable target = vuforia.getTarget(i);
        boolean visible = vuforia.isTargetVisible(target);

        if (SPEECH_ENABLED)
        {
            if (visible != targetsFound[i])
            {
                targetsFound[i] = visible;
                String sentence = String.format(
                        "%s is %s.", target.getName(), visible? "in view": "out of view");
                textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
            }
        }

        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            VectorF translation = pose.getTranslation();
            dashboard.displayPrintf(
                    i + 1, LABEL_WIDTH, target.getName() + " = ", "%6.2f,%6.2f,%6.2f",
                    translation.get(0)/MM_PER_INCH,
                    translation.get(1)/MM_PER_INCH,
                    -translation.get(2)/MM_PER_INCH);
        }

        OpenGLMatrix robotLocation = vuforia.getRobotLocation(target);
        if (robotLocation != null)
        {
            lastKnownRobotLocation = robotLocation;
        }
    }

    if (lastKnownRobotLocation != null)
    {
        dashboard.displayPrintf(5, LABEL_WIDTH, "RobotLoc = ", lastKnownRobotLocation.formatAsTransform());
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.VectorF; //导入依赖的package包/类
public void updateLocation(){

        OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.getListener()).getPose();

        telemetry.addData("Pose", format(pose));



                    /* We further illustrate how to decompose the pose into useful rotational and

                 * translational components */

        if (pose != null) {

            VectorF trans = pose.getTranslation();

            Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);



            // Extract the X, Y, and Z components of the offset of the target relative to the robot

            tX = trans.get(0);

            tY = trans.get(1);

            tZ = trans.get(2);



            // Extract the rotational components of the target relative to the robot

            rX = rot.firstAngle;

            rY = rot.secondAngle;

            rZ = rot.thirdAngle;

            // Z is forward-backward
            //x is sideways
        }

    }