mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
71 lines
2.5 KiB
C++
71 lines
2.5 KiB
C++
|
// Copyright (c) FIRST and other WPILib contributors.
|
||
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
||
|
|
// the WPILib BSD license file in the root directory of this project.
|
||
|
|
|
||
|
|
#include <jni.h>
|
||
|
|
|
||
|
|
#include <wpi/jni_util.h>
|
||
|
|
|
||
|
|
#include "edu_wpi_first_math_WPIMathJNI.h"
|
||
|
|
#include "frc/geometry/Pose3d.h"
|
||
|
|
|
||
|
|
using namespace wpi::java;
|
||
|
|
|
||
|
|
extern "C" {
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Class: edu_wpi_first_math_WPIMathJNI
|
||
|
|
* Method: expPose3d
|
||
|
|
* Signature: (DDDDDDDDDDDDD)[D
|
||
|
|
*/
|
||
|
|
JNIEXPORT jdoubleArray JNICALL
|
||
|
|
Java_edu_wpi_first_math_WPIMathJNI_expPose3d
|
||
|
|
(JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ,
|
||
|
|
jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz,
|
||
|
|
jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
|
||
|
|
jdouble twistRy, jdouble twistRz)
|
||
|
|
{
|
||
|
|
frc::Pose3d pose{
|
||
|
|
units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ},
|
||
|
|
frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}};
|
||
|
|
frc::Twist3d twist{units::meter_t{twistDx}, units::meter_t{twistDy},
|
||
|
|
units::meter_t{twistDz}, units::radian_t{twistRx},
|
||
|
|
units::radian_t{twistRy}, units::radian_t{twistRz}};
|
||
|
|
|
||
|
|
frc::Pose3d result = pose.Exp(twist);
|
||
|
|
|
||
|
|
const auto& resultQuaternion = result.Rotation().GetQuaternion();
|
||
|
|
return MakeJDoubleArray(
|
||
|
|
env, {{result.X().value(), result.Y().value(), result.Z().value(),
|
||
|
|
resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
|
||
|
|
resultQuaternion.Z()}});
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Class: edu_wpi_first_math_WPIMathJNI
|
||
|
|
* Method: logPose3d
|
||
|
|
* Signature: (DDDDDDDDDDDDDD)[D
|
||
|
|
*/
|
||
|
|
JNIEXPORT jdoubleArray JNICALL
|
||
|
|
Java_edu_wpi_first_math_WPIMathJNI_logPose3d
|
||
|
|
(JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ,
|
||
|
|
jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz,
|
||
|
|
jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,
|
||
|
|
jdouble endQy, jdouble endQz)
|
||
|
|
{
|
||
|
|
frc::Pose3d startPose{
|
||
|
|
units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ},
|
||
|
|
frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}};
|
||
|
|
frc::Pose3d endPose{
|
||
|
|
units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
|
||
|
|
frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
|
||
|
|
|
||
|
|
frc::Twist3d result = startPose.Log(endPose);
|
||
|
|
|
||
|
|
return MakeJDoubleArray(
|
||
|
|
env, {{result.dx.value(), result.dy.value(), result.dz.value(),
|
||
|
|
result.rx.value(), result.ry.value(), result.rz.value()}});
|
||
|
|
}
|
||
|
|
|
||
|
|
} // extern "C"
|