void rotate(final int xNew, final int yNew) {
if (content == null || content.isLocked()) return;
final int dx = xNew - xLast;
final int dy = yNew - yLast;
aaX.set(axisPerDx, dx * anglePerPix);
aaY.set(axisPerDy, dy * anglePerPix);
transX.set(aaX);
transY.set(aaY);
rotateTG.getTransform(rotateOld);
rotateNew.set(transl_inv);
rotateNew.mul(transY);
rotateNew.mul(transX);
rotateNew.mul(transl);
rotateNew.mul(rotateOld);
rotateTG.setTransform(rotateNew);
xLast = xNew;
yLast = yNew;
transformChanged(BehaviorCallback.ROTATE, rotateNew);
}
protected void setPinTransform3D(RigidBodyTransform t1, Vector3d u_i, double rotAng) {
t1.setIdentity();
axisAngle.set(u_i, rotAng);
t1.setRotation(axisAngle);
// t1.setRotation();
}
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);
}
private void computeProportionalTerm(FrameOrientation desiredOrientation) {
desiredOrientation.changeFrame(bodyFrame);
desiredOrientation.getQuaternion(errorQuaternion);
errorAngleAxis.set(errorQuaternion);
errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle()));
// Limit the maximum position error considered for control action
double maximumError = gains.getMaximumProportionalError();
if (errorAngleAxis.getAngle() > maximumError) {
errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError);
}
proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ());
proportionalTerm.scale(errorAngleAxis.getAngle());
rotationErrorInBody.set(proportionalTerm);
proportionalGainMatrix.transform(proportionalTerm.getVector());
}
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;
}
}
}
/**
* Returns a transformation matrix that rotates refPoints to match coordPoints
*
* @param refPoints the points to be aligned
* @param referenceVectors
* @return
*/
private Matrix4d alignAxes(Vector3d[] axisVectors, Vector3d[] referenceVectors) {
Matrix4d m1 = new Matrix4d();
AxisAngle4d a = new AxisAngle4d();
Vector3d axis = new Vector3d();
// calculate rotation matrix to rotate refPoints[0] into coordPoints[0]
Vector3d v1 = new Vector3d(axisVectors[0]);
Vector3d v2 = new Vector3d(referenceVectors[0]);
double dot = v1.dot(v2);
if (Math.abs(dot) < 0.999) {
axis.cross(v1, v2);
axis.normalize();
a.set(axis, v1.angle(v2));
m1.set(a);
// make sure matrix element m33 is 1.0. It's 0 on Linux.
m1.setElement(3, 3, 1.0);
} else if (dot > 0) {
// parallel axis, nothing to do -> identity matrix
m1.setIdentity();
} else if (dot < 0) {
// anti-parallel axis, flip around x-axis
m1.set(flipX());
}
// apply transformation matrix to all refPoints
m1.transform(axisVectors[0]);
m1.transform(axisVectors[1]);
// calculate rotation matrix to rotate refPoints[1] into coordPoints[1]
v1 = new Vector3d(axisVectors[1]);
v2 = new Vector3d(referenceVectors[1]);
Matrix4d m2 = new Matrix4d();
dot = v1.dot(v2);
if (Math.abs(dot) < 0.999) {
axis.cross(v1, v2);
axis.normalize();
a.set(axis, v1.angle(v2));
m2.set(a);
// make sure matrix element m33 is 1.0. It's 0 on Linux.
m2.setElement(3, 3, 1.0);
} else if (dot > 0) {
// parallel axis, nothing to do -> identity matrix
m2.setIdentity();
} else if (dot < 0) {
// anti-parallel axis, flip around z-axis
m2.set(flipZ());
}
// apply transformation matrix to all refPoints
m2.transform(axisVectors[0]);
m2.transform(axisVectors[1]);
// combine the two rotation matrices
m2.mul(m1);
// the RMSD should be close to zero
Point3d[] axes = new Point3d[2];
axes[0] = new Point3d(axisVectors[0]);
axes[1] = new Point3d(axisVectors[1]);
Point3d[] ref = new Point3d[2];
ref[0] = new Point3d(referenceVectors[0]);
ref[1] = new Point3d(referenceVectors[1]);
if (SuperPosition.rmsd(axes, ref) > 0.1) {
System.out.println(
"Warning: AxisTransformation: axes alignment is off. RMSD: "
+ SuperPosition.rmsd(axes, ref));
}
return m2;
}
