/**
* Calculate new point(s) X in a B-A system to form B-A-X. Use C as reference for * staggering
* about the B-A bond
*
* <p>(1a) 1 ligand(B) of refAtom (A) which itself has a ligand (C) (i) 1 points required; vector
* along AB vector (ii) 2 points: 2 vectors in ABC plane, staggered and eclipsed wrt C (iii) 3
* points: 1 staggered wrt C, the others +- gauche wrt C If C is null, a random non-colinear C is
* generated
*
* @param aPoint to which substituents are added
* @param nwanted number of points to calculate (1-3)
* @param length A-X length
* @param angle B-A-X angle
* @return Point3d[] nwanted points (or zero if failed)
*/
public static Point3d[] calculate3DCoordinates1(
Point3d aPoint, Point3d bPoint, Point3d cPoint, int nwanted, double length, double angle) {
Point3d points[] = new Point3d[nwanted];
// BA vector
Vector3d ba = new Vector3d(aPoint);
ba.sub(bPoint);
ba.normalize();
// if no cPoint, generate a random reference
if (cPoint == null) {
Vector3d cVector = getNonColinearVector(ba);
cPoint = new Point3d(cVector);
}
// CB vector
Vector3d cb = new Vector3d(bPoint);
cb.sub(cPoint);
cb.normalize();
// if A, B, C colinear, replace C by random point
double cbdotba = cb.dot(ba);
if (cbdotba > 0.999999) {
Vector3d cVector = getNonColinearVector(ba);
cPoint = new Point3d(cVector);
cb = new Vector3d(bPoint);
cb.sub(cPoint);
}
// cbxba = c x b
Vector3d cbxba = new Vector3d();
cbxba.cross(cb, ba);
cbxba.normalize();
// create three perp axes ba, cbxba, and ax
Vector3d ax = new Vector3d();
ax.cross(cbxba, ba);
ax.normalize();
double drot = Math.PI * 2.0 / (double) nwanted;
for (int i = 0; i < nwanted; i++) {
double rot = (double) i * drot;
points[i] = new Point3d(aPoint);
Vector3d vx = new Vector3d(ba);
vx.scale(-Math.cos(angle) * length);
Vector3d vy = new Vector3d(ax);
vy.scale(Math.cos(rot) * length);
Vector3d vz = new Vector3d(cbxba);
vz.scale(Math.sin(rot) * length);
points[i].add(vx);
points[i].add(vy);
points[i].add(vz);
}
return points;
}
public void setViewingDirection(double yaw, double pitch) {
_viewingDirection.set(
Math.sin(Math.toRadians(yaw)) * Math.cos(Math.toRadians(pitch)),
-Math.sin(Math.toRadians(pitch)),
-Math.cos(Math.toRadians(pitch)) * Math.cos(Math.toRadians(yaw)));
_viewingDirection.normalize(_viewingDirection);
}
/**
* Return a midpoint of a helix, calculated from three positions of three adjacent subunit
* centers.
*
* @param p1 center of first subunit
* @param p2 center of second subunit
* @param p3 center of third subunit
* @return midpoint of helix
*/
private Point3d getMidPoint(Point3d p1, Point3d p2, Point3d p3) {
Vector3d v1 = new Vector3d();
v1.sub(p1, p2);
Vector3d v2 = new Vector3d();
v2.sub(p3, p2);
Vector3d v3 = new Vector3d();
v3.add(v1);
v3.add(v2);
v3.normalize();
// calculat the total distance between to subunits
double dTotal = v1.length();
// calculate the rise along the y-axis. The helix axis is aligned with y-axis,
// therfore, the rise between subunits is the y-distance
double rise = p2.y - p1.y;
// use phythagorean theoremm to calculate chord length between two subunit centers
double chord = Math.sqrt(dTotal * dTotal - rise * rise);
// System.out.println("Chord d: " + dTotal + " rise: " + rise + "chord: " + chord);
double angle = helixLayers.getByLargestContacts().getAxisAngle().angle;
// using the axis angle and the chord length, we can calculate the radius of the helix
// http://en.wikipedia.org/wiki/Chord_%28geometry%29
double radius = chord / Math.sin(angle / 2) / 2; // can this go to zero?
// System.out.println("Radius: " + radius);
// project the radius onto the vector that points toward the helix axis
v3.scale(radius);
v3.add(p2);
// System.out.println("Angle: " +
// Math.toDegrees(helixLayers.getByLowestAngle().getAxisAngle().angle));
Point3d cor = new Point3d(v3);
return cor;
}
/**
* Set up projection to plane in q-space
*
* @param qNormal normal to projection plane
* @param qRow vector to describe direction of a row
* @param qDeltas array of pixel sizes in q-space
* @param rowLimits array containing start and (exclusive) stop values of row index
* @param colLimits array containing start and (exclusive) stop values of columns index
*/
public ProjectToQSpacePlane(
Vector3d qNormal, Vector3d qRow, double[] qDeltas, int[] rowLimits, int[] colLimits) {
qNormal.normalize();
row = qRow;
row.normalize();
col = new Vector3d();
col.cross(qNormal, col);
col.normalize();
rdel = qDeltas[0];
cdel = qDeltas[1];
pshape = new int[2];
pshape[0] = rowLimits[1] - rowLimits[0];
pshape[1] = colLimits[1] - colLimits[0];
roff = rowLimits[0];
coff = colLimits[0];
}
/** Set up coordinate system, return if successful */
public boolean setupCS(Lineage refLin) {
// NucLineage.Nuc nucABp = refLin.nuc.get("ABp");
Lineage.Particle nucEMS = refLin.particle.get("EMS");
Vector3d posABp = LineageMergeUtil.getLastPosABp(refLin);
if (posABp == null || nucEMS == null || nucEMS.pos.isEmpty()) {
System.out.println("Does not have enough cells marked, will not produce LR");
return false;
} else System.out.println("Will do LR");
Vector3d posEMS = nucEMS.pos.get(nucEMS.pos.lastKey()).getPosCopy();
ImVector2d dirvec = ImVector2d.polar(shell.major, shell.angle);
Vector3d axisAP = new Vector3d(dirvec.x, dirvec.y, 0);
// Approximate axis - but I want it perpendicular to the AP-axis defined by the shell
Vector3d approxDV = new Vector3d();
approxDV.sub(posEMS, posABp);
Vector3d axisLR = new Vector3d();
axisLR.cross(approxDV, axisAP);
axisLR.normalize();
double axisLength = 2 * shell.minor;
axisLR.scale(1.0 / axisLength);
this.axis = axisLR;
return true;
}
@Override
public void computeTransform(RigidBodyTransform currXform) {
update();
CameraMountInterface cameraMount = getCameraMount();
if (isMounted() && (cameraMount != null)) {
cameraMount.getTransformToCamera(currXform);
return;
}
positionOffset.set(getCamX(), getCamY(), getCamZ());
xAxis.set(getFixX(), getFixY(), getFixZ());
fixPointNode.translateTo(getFixX(), getFixY(), getFixZ());
xAxis.sub(positionOffset);
xAxis.normalize();
zAxis.set(0.0, 0.0, 1.0);
yAxis.cross(zAxis, xAxis);
zAxis.cross(xAxis, yAxis);
rotationMatrix.setColumn(0, xAxis);
rotationMatrix.setColumn(1, yAxis);
rotationMatrix.setColumn(2, zAxis);
currXform.setRotationAndZeroTranslation(rotationMatrix);
currXform.setTranslation(positionOffset);
currXform.normalizeRotationPart();
}
public void lookAt(Vector3d target) {
Vector3d targetDirection = new Vector3d();
targetDirection.sub(target, getPosition());
targetDirection.normalize();
setPitchYawFromVector(targetDirection);
}
/**
* Calculates the shading in camera space
*
* @param p The hit point on the surface in camera space.
* @param n The surface normal at the hit point in camera space.
* @param eye The eye point in camera space.
* @return
*/
protected Color3f shade(Point3d p, Vector3d n, Point3d eye) {
// normalize only if point is not singular
float nLength = (float) n.length();
if (nLength != 0.0f) n.scale(1.0f / nLength);
// compute view vector
Vector3d v = new Vector3d(eye);
v.sub(p);
v.normalize();
/*
// special coloring for blowup-visualization
if( n.dot( v ) < 0.0f )
n.negate();
if( blowUpChooseMaterial( dcsd.rayCreator.cameraSpaceToSurfaceSpace( p ) ) )
{
return shadeWithMaterial( p, v, n, dcsd.frontAmbientColor, dcsd.frontLightProducts );
}
else
{
return shadeWithMaterial( p, v, n, dcsd.backAmbientColor, dcsd.backLightProducts );
}
*/
// compute, which material to use
if (n.dot(v) > 0.0f) {
return shadeWithMaterial(p, v, n, dcsd.frontAmbientColor, dcsd.frontLightProducts);
} else {
n.negate();
return shadeWithMaterial(p, v, n, dcsd.backAmbientColor, dcsd.backLightProducts);
}
}
public void perturb(Vector3d direction) {
Vector3d force = new Vector3d(direction);
if (direction.lengthSquared() > 0.0) {
force.normalize();
force.scale(disturbanceMagnitude.getDoubleValue());
forcePerturbable.setForcePerturbance(force, disturbanceDuration.getDoubleValue());
}
}
@Override
public GroundProfile3D getGroundProfile() {
Vector3d surfaceNormal = new Vector3d(0.03, 0.27, 1.0);
surfaceNormal.normalize();
Point3d intersectionPoint = new Point3d(0.22, 2.2, -0.4);
double maxXY = 10.0;
return new SlopedPlaneGroundProfile(surfaceNormal, intersectionPoint, maxXY);
}
// static helper method
private static Vector3d getNormalizedVector(Vector3d v, Vector3d out) {
out.set(v);
out.normalize();
if (out.length() < BulletGlobals.SIMD_EPSILON) {
out.set(0, 0, 0);
}
return out;
}
boolean check(final ClosestPointPair cpair) {
final Vector3d del = new Vector3d();
del.sub(cpair.pnt2, cpair.pnt1);
if (Math.abs(del.length() - d) > TOL) return false;
del.normalize();
if (!vec.epsilonEquals(del, TOL)) return false;
return true;
}
private void setCameraPosition() {
eye.set(0, 50, zTilt);
eye.normalize();
eye.scale(height);
eyePoint.set(eye);
// NOTE z axis is "up"
up.set(0, 0, (zTilt < 0 ? 1 : -1));
hoverTrans.lookAt(eyePoint, center, up);
hoverTrans.invert();
hoverTransformGroup.setTransform(hoverTrans);
}
/**
* Shades a point with the same algorithm used by the {@link <a
* href="http://surf.sourceforge.net">surf raytracer</a>}.
*
* @param hitPoint Intersection point.
* @param v View vector (from intersection point to eye).
* @param n Surface normal.
* @param material Surface material.
* @return
*/
protected Color3f shadeWithMaterial(
Point3d hitPoint,
Vector3d v,
Vector3d n,
Color3f ambientColor,
LightProducts[] lightProducts) {
Vector3d l = new Vector3d();
Vector3d h = new Vector3d();
Color3f color = new Color3f(ambientColor);
for (int i = 0; i < dcsd.lightSources.length; i++) {
LightSource lightSource = dcsd.lightSources[i];
l.sub(lightSource.getPosition(), hitPoint);
l.normalize();
float lambertTerm = (float) n.dot(l);
if (lambertTerm > 0.0f) {
// compute diffuse color component
color.scaleAdd(lambertTerm, lightProducts[i].getDiffuseProduct(), color);
// compute specular color component
h.add(l, v);
h.normalize();
color.scaleAdd(
(float)
Math.pow(Math.max(0.0f, n.dot(h)), lightProducts[i].getMaterial().getShininess()),
lightProducts[i].getSpecularProduct(),
color);
}
}
color.clampMax(1.0f);
return color;
}
private Vector3d orthogonalize(Vector3d vector1, Vector3d vector2) {
double dot = vector1.dot(vector2);
Vector3d ref = new Vector3d(vector2);
// System.out.println("p.r: " + dot);
// System.out.println("Orig refVector: " + referenceVector);
if (dot < 0) {
vector2.negate();
}
if (Math.abs(dot) < 0.00001) {
System.out.println("HelixAxisAligner: Warning: reference axis parallel");
}
vector2.cross(vector1, vector2);
// System.out.println("Intermed. refVector: " + vector2);
vector2.normalize();
// referenceVector.cross(referenceVector, principalRotationVector);
vector2.cross(vector1, vector2);
vector2.normalize();
if (ref.dot(vector2) < 0) {
vector2.negate();
}
// System.out.println("Mod. refVector: " + vector2);
return vector2;
}
/**
* Calculate new point(s) X in a B-A-C system. It forms form a B-A(-C)-X system.
*
* <p>(2) 2 ligands(B, C) of refAtom A (i) 1 points required; vector in ABC plane bisecting AB,
* AC. If ABC is linear, no points (ii) 2 points: 2 points X1, X2, X1-A-X2 = angle about 2i vector
*
* @param aPoint to which substituents are added
* @param bPoint first ligand of A
* @param cPoint second ligand of A
* @param nwanted number of points to calculate (1-2)
* @param length A-X length
* @param angle B-A-X angle
* @return Point3d[] nwanted points (or zero if failed)
*/
public static Point3d[] calculate3DCoordinates2(
Point3d aPoint, Point3d bPoint, Point3d cPoint, int nwanted, double length, double angle) {
Point3d newPoints[] = new Point3d[0];
double ang2 = angle / 2.0;
Vector3d ba = new Vector3d(aPoint);
ba.sub(bPoint);
Vector3d ca = new Vector3d(aPoint);
ca.sub(cPoint);
Vector3d baxca = new Vector3d();
baxca.cross(ba, ca);
if (baxca.length() < 0.00000001) {; // linear
} else if (nwanted == 1) {
newPoints = new Point3d[1];
Vector3d ax = new Vector3d(ba);
ax.add(ca);
ax.normalize();
ax.scale(length);
newPoints[0] = new Point3d(aPoint);
newPoints[0].add(ax);
} else if (nwanted == 2) {
newPoints = new Point3d[2];
Vector3d ax = new Vector3d(ba);
ax.add(ca);
ax.normalize();
baxca.normalize();
baxca.scale(Math.sin(ang2) * length);
ax.scale(Math.cos(ang2) * length);
newPoints[0] = new Point3d(aPoint);
newPoints[0].add(ax);
newPoints[0].add(baxca);
newPoints[1] = new Point3d(aPoint);
newPoints[1].add(ax);
newPoints[1].sub(baxca);
}
return newPoints;
}
/**
* Rescales Point2 so that length 1-2 is sum of covalent radii. if covalent radii cannot be found,
* use bond length of 1.0
*
* @param atom1 stationary atom
* @param atom2 moveable atom
* @param point2 coordinates for atom 2
* @return new coords for atom 2
*/
public static Point3d rescaleBondLength(IAtom atom1, IAtom atom2, Point3d point2) {
Point3d point1 = atom1.getPoint3d();
double d1 = atom1.getCovalentRadius();
double d2 = atom2.getCovalentRadius();
// in case we have no covalent radii, set to 1.0
double distance =
(d1 < 0.1 || d2 < 0.1) ? 1.0 : atom1.getCovalentRadius() + atom2.getCovalentRadius();
Vector3d vect = new Vector3d(point2);
vect.sub(point1);
vect.normalize();
vect.scale(distance);
Point3d newPoint = new Point3d(point1);
newPoint.add(vect);
return newPoint;
}
@SuppressWarnings("all")
protected void updateTargetPositionBasedOnCollision(
Vector3d hitNormal, double tangentMag, double normalMag) {
Vector3d movementDirection = new Vector3d();
movementDirection.sub(targetPosition, currentPosition);
double movementLength = movementDirection.length();
if (movementLength > BulletGlobals.SIMD_EPSILON) {
movementDirection.normalize();
Vector3d reflectDir =
computeReflectionDirection(movementDirection, hitNormal, new Vector3d());
reflectDir.normalize();
Vector3d parallelDir = parallelComponent(reflectDir, hitNormal, new Vector3d());
Vector3d perpindicularDir = perpindicularComponent(reflectDir, hitNormal, new Vector3d());
targetPosition.set(currentPosition);
if (false) // tangentMag != 0.0)
{
Vector3d parComponent = new Vector3d();
parComponent.scale(tangentMag * movementLength, parallelDir);
// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
targetPosition.add(parComponent);
}
if (normalMag != 0.0f) {
Vector3d perpComponent = new Vector3d();
perpComponent.scale(normalMag * movementLength, perpindicularDir);
// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
targetPosition.add(perpComponent);
}
} else {
// printf("movementLength don't normalize a zero vector\n");
}
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testChainAgainstCentroidalMomentumMatrix() {
Random random = new Random(17679L);
ArrayList<RevoluteJoint> joints = new ArrayList<RevoluteJoint>();
RigidBody elevator = new RigidBody("elevator", world);
int nJoints = 10;
Vector3d[] jointAxes = new Vector3d[nJoints];
for (int i = 0; i < nJoints; i++) {
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
jointAxes[i] = jointAxis;
}
ScrewTestTools.createRandomChainRobot("randomChain", joints, elevator, jointAxes, random);
InverseDynamicsJoint[] jointsArray = new RevoluteJoint[joints.size()];
joints.toArray(jointsArray);
ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator);
for (RevoluteJoint joint : joints) {
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
}
Momentum momentum = computeMomentum(elevator, centerOfMassFrame);
DenseMatrix64F momentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
momentum.getMatrix(momentumMatrix);
CentroidalMomentumMatrix centroidalMomentumMatrix =
new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
centroidalMomentumMatrix.compute();
DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getMatrix();
DenseMatrix64F jointVelocitiesMatrix =
new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jointsArray), 1);
ScrewTools.getJointVelocitiesMatrix(jointsArray, jointVelocitiesMatrix);
DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
CommonOps.mult(
centroidalMomentumMatrixMatrix,
jointVelocitiesMatrix,
momentumFromCentroidalMomentumMatrix);
double epsilon = 1e-9;
assertEquals(momentum.getExpressedInFrame(), centerOfMassFrame);
JUnitTools.assertMatrixEquals(momentumFromCentroidalMomentumMatrix, momentumMatrix, epsilon);
double norm = NormOps.normP2(momentumMatrix);
assertTrue(norm > epsilon);
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyRotation() {
Random random = new Random(1766L);
RigidBody elevator = new RigidBody("elevator", world);
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
RigidBodyTransform transformToParent = new RigidBodyTransform();
transformToParent.setIdentity();
RevoluteJoint joint =
ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
RigidBody body =
ScrewTools.addRigidBody(
"body",
joint,
RandomTools.generateRandomDiagonalMatrix3d(random),
random.nextDouble(),
new Vector3d());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector linearMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
FrameVector angularMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());
FrameVector linearMomentumCheck = new FrameVector(world);
Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
angularMomentumCheckVector.scale(joint.getQd());
inertia.transform(angularMomentumCheckVector);
FrameVector angularMomentumCheck =
new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
angularMomentumCheck.changeFrame(world);
double epsilon = 1e-9;
JUnitTools.assertTuple3dEquals(
linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
JUnitTools.assertTuple3dEquals(
angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
assertTrue(angularMomentum.length() > epsilon);
}
public void move() {
Vector3d velocity = new Vector3d(cameraXP - cameraXN, 0, cameraZP - cameraZN);
velocity.normalize();
if (Double.isNaN(velocity.x)) {
velocity = new Vector3d();
}
position.add(velocity);
if (position.x < -maxX) {
position.x = -maxX;
} else if (position.x > maxX) {
position.x = maxX;
}
if (position.z < -maxZ) {
position.z = -maxZ;
} else if (position.z > maxZ) {
position.z = maxZ;
}
}
@Override
public void performEffect(EntityLivingBase entity, int id) {
// super magnetic - inspired by botanias code
double x = entity.posX;
double y = entity.posY;
double z = entity.posZ;
double range = 1.8d;
range += entity.getActivePotionEffect(this).getAmplifier() * 0.3f;
List<EntityItem> items =
entity.worldObj.getEntitiesWithinAABB(
EntityItem.class,
new AxisAlignedBB(x - range, y - range, z - range, x + range, y + range, z + range));
int pulled = 0;
for (EntityItem item : items) {
if (item.getEntityItem() == null || item.getEntityItem().getItem() == null || item.isDead) {
continue;
}
if (pulled > 200) {
break;
}
// constant force!
float strength = 0.07f;
// calculate direction: item -> player
Vector3d vec = new Vector3d(x, y, z);
vec.sub(new Vector3d(item.posX, item.posY, item.posZ));
vec.normalize();
vec.scale(strength);
// we calculated the movement vector and set it to the correct strength.. now we apply it
// \o/
item.motionX += vec.x;
item.motionY += vec.y;
item.motionZ += vec.z;
pulled++;
}
}
/**
* Calculate new point X in a B-A(-D)-C system. It forms a B-A(-D)(-C)-X system.
*
* <p>(3) 3 ligands(B, C, D) of refAtom A (i) 1 points required; if A, B, C, D coplanar, no
* points. else vector is resultant of BA, CA, DA
*
* @param aPoint to which substituents are added
* @param bPoint first ligand of A
* @param cPoint second ligand of A
* @param dPoint third ligand of A
* @param length A-X length
* @return Point3d nwanted points (or null if failed (coplanar))
*/
public static Point3d calculate3DCoordinates3(
Point3d aPoint, Point3d bPoint, Point3d cPoint, Point3d dPoint, double length) {
Vector3d v1 = new Vector3d(aPoint);
v1.sub(bPoint);
Vector3d v2 = new Vector3d(aPoint);
v2.sub(cPoint);
Vector3d v3 = new Vector3d(aPoint);
v3.sub(dPoint);
Vector3d v = new Vector3d(bPoint);
v.add(cPoint);
v.add(dPoint);
if (v.length() < 0.00001) {
return null;
}
v.normalize();
v.scale(length);
Point3d point = new Point3d(aPoint);
point.add(v);
return point;
}
public void interactionForce(Particle other) {
// ok, now for the fun stuff...
Vector3d posDif = new Vector3d();
posDif.sub(x, other.x);
Vector3d velDif = new Vector3d();
velDif.sub(v, other.v);
double d = posDif.length();
double dSquared = d * d;
int m = 6;
int n = 5;
double r0 = 2 * PARTICLE_RADIUS;
double cr = r0;
double cd = r0;
double b1 = 1;
double b2 = b1; // *Math.pow(r0, n-m);
double sumR = 2 * PARTICLE_RADIUS;
double sr = 250;
double sd = 70;
/*sr = dSquared/(cr*cr*(sumR)*(sumR));
sd = dSquared/(cd*cd*(sumR)*(sumR));
sr = Math.max(0, 1 - sr);
sd = Math.max(0, 1 - sd);*/
Vector3d f = new Vector3d();
f.set(posDif);
f.normalize();
double sf =
-sr * (b1 / Math.pow(d / r0, m) - b2 / Math.pow(d / r0, n))
+ sd * (velDif.dot(f) / (d / r0));
f.scale(sf);
other.f.add(f);
}
/** performs the gl camera transformations for the right eye */
public void translateRightEye(GL gl, GLU glu, GLUT glut) {
// calculate the right vector
Vector3d right = new Vector3d();
Vector3d temp1 = (Vector3d) cameraTarget.clone();
temp1.scale(-1);
Vector3d temp2 = (Vector3d) cameraPosition.clone();
temp2.add(temp1);
right.cross(cameraUp, temp2);
right.normalize();
right.scale(eyespread / 2.);
glu.gluLookAt(
right.x + cameraPosition.x,
right.y + cameraPosition.y,
right.z + cameraPosition.z,
right.x + cameraTarget.x,
right.y + cameraTarget.y,
right.z + cameraTarget.z,
cameraUp.x,
cameraUp.y,
cameraUp.z);
}
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);
}
protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
// printf("m_normalizedDirection=%f,%f,%f\n",
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
Transform start = new Transform();
Transform end = new Transform();
targetPosition.add(currentPosition, walkMove);
start.setIdentity();
end.setIdentity();
double fraction = 1.0f;
Vector3d distance2Vec = new Vector3d();
distance2Vec.sub(currentPosition, targetPosition);
double distance2 = distance2Vec.lengthSquared();
// printf("distance2=%f\n",distance2);
/*if (touchingContact) {
if (normalizedDirection.dot(touchingNormal) > 0.0f) {
updateTargetPositionBasedOnCollision(touchingNormal);
}
}*/
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0) {
start.origin.set(currentPosition);
end.origin.set(targetPosition);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(
ghostObject, upAxisDirection[upAxis], -1.0f);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
double margin = convexShape.getMargin();
convexShape.setMargin(margin + addedMargin);
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape,
start,
end,
callback,
collisionWorld.getDispatchInfo().allowedCcdPenetration);
} else {
collisionWorld.convexSweepTest(convexShape, start, end, callback);
}
convexShape.setMargin(margin);
fraction -= callback.closestHitFraction;
if (callback.hasHit()) {
// we moved only a fraction
Vector3d hitDistanceVec = new Vector3d();
hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
// double hitDistance = hitDistanceVec.length();
// if the distance is farther than the collision margin, move
// if (hitDistance > addedMargin) {
// //printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
// currentPosition.interpolate(currentPosition, targetPosition,
// callback.closestHitFraction);
// }
updateTargetPositionBasedOnCollision(callback.hitNormalWorld);
Vector3d currentDir = new Vector3d();
currentDir.sub(targetPosition, currentPosition);
distance2 = currentDir.lengthSquared();
if (distance2 > BulletGlobals.SIMD_EPSILON) {
currentDir.normalize();
// see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
// oscilations in sloping corners."
if (currentDir.dot(normalizedDirection) <= 0.0f) {
break;
}
} else {
// printf("currentDir: don't normalize a zero vector\n");
break;
}
} else {
// we moved whole way
currentPosition.set(targetPosition);
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}
/**
* 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;
}
/**
* Draws the five sides of the hemicube into the provided drawable
*
* @param drawable
* @param which
* @param near
* @param far
* @param bCollect
*/
public void drawHemiCube(
GLAutoDrawable drawable, int which, double near, double far, boolean bCollect) {
// TODO - PART 1 - DRAW HEMICUBE!
GL gl = drawable.getGL();
GLU glu = new GLU();
/* Return the face based on which parameter */
int[] vertices = scene.getObject().getFace(which);
/* Find the center of the face */
/* Center of the face is the average of the three vertices */
/* Using returned list of vertex indices, find the vertices
* corresponding to a particular face */
Vertex v1 = scene.getObject().vertexList.get(vertices[0]);
Vertex v2 = scene.getObject().vertexList.get(vertices[1]);
Vertex v3 = scene.getObject().vertexList.get(vertices[2]);
/* Locate center of face */
/* Average of three vertices */
Point3d centerPoint = new Point3d();
centerPoint = new Point3d(v1.p);
centerPoint.add(v2.p);
centerPoint.add(v3.p);
centerPoint.scale(0.33333);
/* Set up camera frame */
/* --- Surface normal --- */
/* Declare points of vertex for face */
Point3d p1 = new Point3d(v1.p);
Point3d p2 = new Point3d(v2.p);
Point3d p3 = new Point3d(v3.p);
/* Declare vector u as p2-p1 */
Point3d uVec = new Point3d(p2);
uVec.sub(p1);
Vector3d u = new Vector3d(uVec);
/* Declare vector v as p3-p1 */
Point3d vVec = new Point3d(p3);
vVec.sub(p1);
Vector3d v = new Vector3d(vVec);
/* Make normal vector */
Vector3d norm = new Vector3d();
norm.cross(u, v);
/* --- Vectors Orthonormal to Normal --- */
Point3d vec1pt = new Point3d(p1);
vec1pt.sub(p2);
Vector3d vec1 = new Vector3d(vec1pt);
vec1.cross(vec1, norm); // Cross surface normal with vec1 to get orthogonal vector
Vector3d vec2 = new Vector3d();
vec2.cross(
norm,
vec1); // Cross product of surface normal with new vector vec1 to get 2nd orthogonal vector
/* Make unit vectors */
norm.normalize();
vec1.normalize();
vec2.normalize();
/* Set up the five different frustums, and stitch together using viewPort */
/* Viewport to set up the view of the scene */
/* ----- FRONT FACE ----- */
gl.glPushMatrix();
gl.glViewport(0, 0, drawable.getWidth(), drawable.getHeight() / 3);
/* Set up frustums for this face */
gl.glMatrixMode(gl.GL_PROJECTION);
gl.glLoadIdentity();
gl.glFrustum(-near, near, -near, near, near, far);
/* Position camera at center of specified patch (which) */
gl.glMatrixMode(gl.GL_MODELVIEW);
gl.glLoadIdentity();
glu.gluLookAt(
centerPoint.x,
centerPoint.y,
centerPoint.z,
centerPoint.x + norm.x,
centerPoint.y + norm.y,
centerPoint.z + norm.z,
centerPoint.x + vec1.x,
centerPoint.y + vec1.y,
centerPoint.z + vec1.z);
/* Draw the frustum to screen */
draw(drawable, scene.drawStyle.IDENT);
gl.glPopMatrix();
/* ----- BOTTOM FACE ----- */
gl.glPushMatrix();
gl.glViewport(0, drawable.getHeight() / 3, drawable.getWidth(), drawable.getHeight() / 6);
gl.glMatrixMode(gl.GL_PROJECTION);
gl.glLoadIdentity();
gl.glFrustum(-near, near, 0, near, near, far);
/* Position camera at center of specified patch (which) */
gl.glMatrixMode(gl.GL_MODELVIEW);
gl.glLoadIdentity();
glu.gluLookAt(
centerPoint.x,
centerPoint.y,
centerPoint.z,
centerPoint.x + (-vec1.x),
centerPoint.y + (-vec1.y),
centerPoint.z + (-vec1.z),
centerPoint.x + norm.x,
centerPoint.y + norm.y,
centerPoint.z + norm.z);
/* Draw the frustum to screen */
draw(drawable, scene.drawStyle.IDENT);
gl.glPopMatrix();
/* ----- TOP FACE ----- */
gl.glPushMatrix();
gl.glViewport(
0,
(drawable.getHeight() / 3) + (drawable.getHeight() / 6),
drawable.getWidth(),
drawable.getHeight() / 6);
gl.glMatrixMode(gl.GL_PROJECTION);
gl.glLoadIdentity();
gl.glFrustum(-near, near, -near, 0, near, far);
/* Position camera at center of specified patch (which) */
gl.glMatrixMode(gl.GL_MODELVIEW);
gl.glLoadIdentity();
glu.gluLookAt(
centerPoint.x,
centerPoint.y,
centerPoint.z,
centerPoint.x + vec1.x,
centerPoint.y + vec1.y,
centerPoint.z + vec1.z,
centerPoint.x - norm.x,
centerPoint.y - norm.y,
centerPoint.z - norm.z);
/* Draw the frustum to screen */
draw(drawable, scene.drawStyle.IDENT);
gl.glPopMatrix();
/* ----- LEFT FACE ----- */
gl.glPushMatrix();
gl.glViewport(
0,
(drawable.getHeight() / 3) + 2 * (drawable.getHeight() / 6),
drawable.getWidth(),
drawable.getHeight() / 6);
gl.glMatrixMode(gl.GL_PROJECTION);
gl.glLoadIdentity();
gl.glFrustum(0, near, -near, near, near, far);
/* Position camera at center of specified patch (which) */
gl.glMatrixMode(gl.GL_MODELVIEW);
gl.glLoadIdentity();
glu.gluLookAt(
centerPoint.x,
centerPoint.y,
centerPoint.z,
centerPoint.x + vec2.x,
centerPoint.y + vec2.y,
centerPoint.z + vec2.z,
centerPoint.x + vec1.x,
centerPoint.y + vec1.y,
centerPoint.z + vec1.z);
/* Draw the frustum to screen */
draw(drawable, scene.drawStyle.IDENT);
gl.glPopMatrix();
/* ----- RIGHT FACE ----- */
gl.glPushMatrix();
gl.glViewport(
0,
(drawable.getHeight() / 3) + 3 * (drawable.getHeight() / 6),
drawable.getWidth(),
drawable.getHeight() / 6);
gl.glMatrixMode(gl.GL_PROJECTION);
gl.glLoadIdentity();
gl.glFrustum(near, 0, -near, near, near, far);
/* Position camera at center of specified patch (which) */
gl.glMatrixMode(gl.GL_MODELVIEW);
gl.glLoadIdentity();
glu.gluLookAt(
centerPoint.x,
centerPoint.y,
centerPoint.z,
centerPoint.x + (-vec2.x),
centerPoint.y + (-vec2.y),
centerPoint.z + (-vec2.z),
centerPoint.x + vec1.x,
centerPoint.y + vec1.y,
centerPoint.z + vec1.z);
/* Draw the frustum to screen */
draw(drawable, scene.drawStyle.IDENT);
gl.glPopMatrix();
/* ---- End Frustums ---- */
// if collecting the form factors, then read back and process the data
if (bCollect) {
gl.glFlush();
gl.glFinish();
gl.glReadPixels(
0, 0, divisions * 2, divisions * 6, GL.GL_RGBA, GL.GL_UNSIGNED_BYTE, pixelDataBuffer);
collectData(which);
}
}
// called on the parent object
// Should be synchronized so that the user thread does not modify the
// OrientedShape3D params while computing the transform
synchronized void updateOrientedTransform(Canvas3D canvas, int viewIndex) {
double angle = 0.0;
double sign;
boolean status;
Transform3D orientedxform = getOrientedTransform(viewIndex);
// get viewplatforms's location in virutal world
if (mode == OrientedShape3D.ROTATE_ABOUT_AXIS) { // rotate about axis
canvas.getCenterEyeInImagePlate(viewPosition);
canvas.getImagePlateToVworld(xform); // xform is imagePlateToLocal
xform.transform(viewPosition);
// get billboard's transform
xform.set(getCurrentLocalToVworld());
xform.invert(); // xform is now vWorldToLocal
// transform the eye position into the billboard's coordinate system
xform.transform(viewPosition);
// eyeVec is a vector from the local origin to the eye pt in local
eyeVec.set(viewPosition);
eyeVec.normalize();
// project the eye into the rotation plane
status = projectToPlane(eyeVec, nAxis);
if (status) {
// project the z axis into the rotation plane
zAxis.x = 0.0;
zAxis.y = 0.0;
zAxis.z = 1.0;
status = projectToPlane(zAxis, nAxis);
}
if (status) {
// compute the sign of the angle by checking if the cross product
// of the two vectors is in the same direction as the normal axis
vector.cross(eyeVec, zAxis);
if (vector.dot(nAxis) > 0.0) {
sign = 1.0;
} else {
sign = -1.0;
}
// compute the angle between the projected eye vector and the
// projected z
double dot = eyeVec.dot(zAxis);
if (dot > 1.0f) {
dot = 1.0f;
} else if (dot < -1.0f) {
dot = -1.0f;
}
angle = sign * Math.acos(dot);
// use -angle because xform is to *undo* rotation by angle
aa.x = nAxis.x;
aa.y = nAxis.y;
aa.z = nAxis.z;
aa.angle = -angle;
orientedxform.set(aa);
} else {
orientedxform.setIdentity();
}
} else if (mode == OrientedShape3D.ROTATE_ABOUT_POINT) { // rotate about point
// Need to rotate Z axis to point to eye, and Y axis to be
// parallel to view platform Y axis, rotating around rotation pt
// get the eye point
canvas.getCenterEyeInImagePlate(viewPosition);
// derive the yUp point
yUpPoint.set(viewPosition);
yUpPoint.y += 0.01; // one cm in Physical space
// transform the points to the Billboard's space
canvas.getImagePlateToVworld(xform); // xform is ImagePlateToVworld
xform.transform(viewPosition);
xform.transform(yUpPoint);
// get billboard's transform
xform.set(getCurrentLocalToVworld());
xform.invert(); // xform is vWorldToLocal
// transfom points to local coord sys
xform.transform(viewPosition);
xform.transform(yUpPoint);
// Make a vector from viewPostion to 0,0,0 in the BB coord sys
eyeVec.set(viewPosition);
eyeVec.normalize();
// create a yUp vector
yUp.set(yUpPoint);
yUp.sub(viewPosition);
yUp.normalize();
// find the plane to rotate z
zAxis.x = 0.0;
zAxis.y = 0.0;
zAxis.z = 1.0;
// rotation axis is cross product of eyeVec and zAxis
vector.cross(eyeVec, zAxis); // vector is cross product
// if cross product is non-zero, vector is rotation axis and
// rotation angle is acos(eyeVec.dot(zAxis)));
double length = vector.length();
if (length > 0.0001) {
double dot = eyeVec.dot(zAxis);
if (dot > 1.0f) {
dot = 1.0f;
} else if (dot < -1.0f) {
dot = -1.0f;
}
angle = Math.acos(dot);
aa.x = vector.x;
aa.y = vector.y;
aa.z = vector.z;
aa.angle = -angle;
zRotate.set(aa);
} else {
// no rotation needed, set to identity (scale = 1.0)
zRotate.set(1.0);
}
// Transform the yAxis by zRotate
yAxis.x = 0.0;
yAxis.y = 1.0;
yAxis.z = 0.0;
zRotate.transform(yAxis);
// project the yAxis onto the plane perp to the eyeVec
status = projectToPlane(yAxis, eyeVec);
if (status) {
// project the yUp onto the plane perp to the eyeVec
status = projectToPlane(yUp, eyeVec);
}
if (status) {
// rotation angle is acos(yUp.dot(yAxis));
double dot = yUp.dot(yAxis);
// Fix numerical error, otherwise acos return NULL
if (dot > 1.0f) {
dot = 1.0f;
} else if (dot < -1.0f) {
dot = -1.0f;
}
angle = Math.acos(dot);
// check the sign by looking a the cross product vs the eyeVec
vector.cross(yUp, yAxis); // vector is cross product
if (eyeVec.dot(vector) < 0) {
angle *= -1;
}
aa.x = eyeVec.x;
aa.y = eyeVec.y;
aa.z = eyeVec.z;
aa.angle = -angle;
xform.set(aa); // xform is now yRotate
// rotate around the rotation point
vector.x = rotationPoint.x;
vector.y = rotationPoint.y;
vector.z = rotationPoint.z; // vector to translate to RP
orientedxform.set(vector); // translate to RP
orientedxform.mul(xform); // yRotate
orientedxform.mul(zRotate); // zRotate
vector.scale(-1.0); // vector to translate back
xform.set(vector); // xform to translate back
orientedxform.mul(xform); // translate back
} else {
orientedxform.setIdentity();
}
}
// Scale invariant computation
if (constantScale) {
// Back Xform a unit vector to local world coords
canvas.getInverseVworldProjection(left_xform, right_xform);
// the two endpts of the vector have to be transformed
// individually because the Xform is not affine
im_vec[0].set(0.0, 0.0, 0.0, 1.0);
im_vec[1].set(1.0, 0.0, 0.0, 1.0);
left_xform.transform(im_vec[0]);
left_xform.transform(im_vec[1]);
left_xform.set(getCurrentLocalToVworld());
left_xform.invert();
left_xform.transform(im_vec[0]);
left_xform.transform(im_vec[1]);
lvec.set(im_vec[1]);
lvec.sub(im_vec[0]);
// We simply need the direction of this vector
lvec.normalize();
im_vec[0].set(0.0, 0.0, 0.0, 1.0);
im_vec[1].set(lvec);
im_vec[1].w = 1.0;
// Forward Xfrom to clip coords
left_xform.set(getCurrentLocalToVworld());
left_xform.transform(im_vec[0]);
left_xform.transform(im_vec[1]);
canvas.getVworldProjection(left_xform, right_xform);
left_xform.transform(im_vec[0]);
left_xform.transform(im_vec[1]);
// Perspective division
im_vec[0].x /= im_vec[0].w;
im_vec[0].y /= im_vec[0].w;
im_vec[0].z /= im_vec[0].w;
im_vec[1].x /= im_vec[1].w;
im_vec[1].y /= im_vec[1].w;
im_vec[1].z /= im_vec[1].w;
lvec.set(im_vec[1]);
lvec.sub(im_vec[0]);
// Use the length of this vector to determine the scaling
// factor
double scale = 1 / lvec.length();
// Convert to meters
scale *= scaleFactor * canvas.getPhysicalWidth() / 2;
// Scale object so that it appears the same size
scaleXform.setScale(scale);
orientedxform.mul(scaleXform);
}
}