public double resolveCenterOfPressureAndNormalTorque(
FramePoint centerOfPressureToPack,
SpatialForceVector spatialForceVector,
ReferenceFrame centerOfPressurePlaneFrame) {
// First resolve the wrench at the plane origin:
wrenchResolvedOnPlane.set(spatialForceVector);
wrenchResolvedOnPlane.changeFrame(centerOfPressurePlaneFrame);
wrenchResolvedOnPlane.getAngularPart(torqueAtZeroInPlaneFrame);
wrenchResolvedOnPlane.getLinearPart(forceInPlaneFrame);
double fz = forceInPlaneFrame.getZ();
double vector12x = Double.NaN;
double vector12y = Double.NaN;
double normalTorqueAtCenterOfPressure;
if (fz > 1e-7) {
// with sufficient normal force
vector12x = -1.0 / fz * torqueAtZeroInPlaneFrame.getY();
vector12y = 1.0 / fz * torqueAtZeroInPlaneFrame.getX();
normalTorqueAtCenterOfPressure =
torqueAtZeroInPlaneFrame.getZ()
- vector12x * forceInPlaneFrame.getY()
+ vector12y * forceInPlaneFrame.getX();
} else {
// without normal force
normalTorqueAtCenterOfPressure = torqueAtZeroInPlaneFrame.getZ();
}
centerOfPressureToPack.setIncludingFrame(centerOfPressurePlaneFrame, vector12x, vector12y, 0.0);
return normalTorqueAtCenterOfPressure;
}
public void doMouseDraggedRight(double dx, double dy) {
// Elevate up and down
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
// double offsetDistance = v3d.length();
t3d.setRotationYawAndZeroTranslation(-dx * rotate_camera_factor);
t3d.transform(v3d);
if (!isTracking || (!isTrackingX && !isTrackingY)) {
fixX = camX - v3d.getX();
fixY = camY - v3d.getY();
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
// v3d.set(delX0, delY0, delZ0);
rotVector.set(-1.0, 0.0, 0.0);
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_camera_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isTracking || (!isTrackingX && !isTrackingY)) {
fixX = camX - v3d.getX();
fixY = camY - v3d.getY();
}
if (!isTracking || !isTrackingZ) {
fixZ = camZ - v3d.getZ();
/*
* double factor = elevate_camera_factor * offsetDistance;
* //Math.abs(camZ-fixZ); if (factor < elevate_camera_factor) factor
* = elevate_camera_factor; fixZ = camZ - v3d.z + dy factor; //fixZ
* = camZ - v3d.z + dy * elevate_factor;
*/
}
}
// transformChanged(currXform);
}
public void doMouseDraggedLeft(double dx, double dy) {
// Rotate around fix point:
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
// double offsetDistance = v3d.length();
t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
t3d.transform(v3d);
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
// v3d.set(delX0, delY0, delZ0);
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
if (!isDolly || !isDollyZ) {
camZ = v3d.getZ() + fixZ;
/*
* double factor = elevate_factor * Math.abs(offsetDistance);
* //camZ-fixZ); if (factor < elevate_factor) factor =
* elevate_factor; //camZ = v3d.z + fixZ + dy * elevate_factor; camZ
* = v3d.z + fixZ + dy * factor;
*/
}
}
// transformChanged(currXform);
}
public void rotateAroundFix(double dx, double dy) {
double distanceFromCameraToFix =
Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camZ - fixZ, 2));
if (distanceFromCameraToFix > 1.0) {
dx /= distanceFromCameraToFix;
dy /= distanceFromCameraToFix;
}
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
t3d.transform(v3d);
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
if (!isDolly || !isDollyZ) {
camZ = v3d.getZ() + fixZ;
}
}
}
public void doMouseDraggedMiddle(double dx, double dy) {
// Zooms in and out
if ((this.isMounted) && (viewportAdapter != null)) {
cameraMount.zoom(dy * 0.1);
} else {
Vector3d v3d = new Vector3d(camX - fixX, camY - fixY, camZ - fixZ);
Vector3d offsetVec = new Vector3d(v3d);
// offsetVec.normalize();
offsetVec.scale(dy * zoom_factor);
// if (offsetVec.length() < v3d.length())
// {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX += offsetVec.getX();
camY += offsetVec.getY();
}
if (!isDolly || !isDollyZ) camZ += offsetVec.getZ();
// }
v3d.set(camX - fixX, camY - fixY, camZ - fixZ);
if (v3d.length() < MIN_CAMERA_POSITION_TO_FIX_DISTANCE) {
v3d.normalize();
v3d.scale(MIN_CAMERA_POSITION_TO_FIX_DISTANCE);
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
camZ = v3d.getZ() + fixZ;
}
}
// transformChanged(currXform);
}
public void drawBox(BufferedImage image, RigidBodyTransform transform, double scale) {
DenseMatrix64F rotation = new DenseMatrix64F(3, 3);
Vector3d translation = new Vector3d();
transform.getRotation(rotation);
transform.getTranslation(translation);
Se3_F64 targetToSensor =
new Se3_F64(
rotation, new Vector3D_F64(translation.getX(), translation.getY(), translation.getZ()));
Graphics2D g2 = image.createGraphics();
g2.setStroke(new BasicStroke(4));
g2.setColor(java.awt.Color.RED);
VisualizeFiducial.drawCube(targetToSensor, this.intrinsicParameters, scale, 2, g2);
g2.finalize();
}
public void setAngularAccelerationInBody(Vector3d angularAccelerationInBody) {
qdd_wx.set(angularAccelerationInBody.getX());
qdd_wy.set(angularAccelerationInBody.getY());
qdd_wz.set(angularAccelerationInBody.getZ());
}
public void setAngularVelocityInBody(Vector3d angularVelocityInBody) {
qd_wx.set(angularVelocityInBody.getX());
qd_wy.set(angularVelocityInBody.getY());
qd_wz.set(angularVelocityInBody.getZ());
}
public double getBodyVelX(Vector3d linearVelocityToPack) {
bodyJointID.getLinearVelocity(linearVelocityToPack);
double velX = linearVelocityToPack.getX();
return velX;
}
public static void packVector3dToGeometry_MsgPoint(Vector3d position, Point point) {
point.setX(position.getX());
point.setY(position.getY());
point.setZ(position.getZ());
}
public static void packVector3dToGeometry_msgsVector3(Vector3d vector, Vector3 rosVectorToPack) {
rosVectorToPack.setX(vector.getX());
rosVectorToPack.setY(vector.getY());
rosVectorToPack.setZ(vector.getZ());
}