mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
43 lines
1.3 KiB
C++
43 lines
1.3 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 "org_wpilib_math_jni_Twist3dJNI.h"
|
|
#include "wpi/math/geometry/Transform3d.hpp"
|
|
#include "wpi/math/geometry/Twist3d.hpp"
|
|
#include "wpi/units/angle.hpp"
|
|
#include "wpi/units/length.hpp"
|
|
#include "wpi/util/jni_util.hpp"
|
|
|
|
using namespace wpi::util::java;
|
|
|
|
extern "C" {
|
|
|
|
/*
|
|
* Class: org_wpilib_math_jni_Twist3dJNI
|
|
* Method: exp
|
|
* Signature: (DDDDDD)[D
|
|
*/
|
|
JNIEXPORT jdoubleArray JNICALL
|
|
Java_org_wpilib_math_jni_Twist3dJNI_exp
|
|
(JNIEnv* env, jclass, jdouble twistDx, jdouble twistDy, jdouble twistDz,
|
|
jdouble twistRx, jdouble twistRy, jdouble twistRz)
|
|
{
|
|
wpi::math::Twist3d twist{
|
|
wpi::units::meter_t{twistDx}, wpi::units::meter_t{twistDy},
|
|
wpi::units::meter_t{twistDz}, wpi::units::radian_t{twistRx},
|
|
wpi::units::radian_t{twistRy}, wpi::units::radian_t{twistRz}};
|
|
|
|
wpi::math::Transform3d result = twist.Exp();
|
|
|
|
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()}});
|
|
}
|
|
|
|
} // extern "C"
|