private boolean matchPair(final ClosestPointPair cpair, final ClosestPointPair check) {
if (check.dist <= 0) { // then just verify collison
if (cpair.dist > 0) return false;
} else if (check.feat1 == null) {
final Vector3d del = new Vector3d();
del.sub(cpair.pnt2, cpair.pnt1);
// then just check distance and direction
if (Math.abs(check.dist - cpair.dist) > TOL || Math.abs(del.length() - check.dist) > TOL)
return false;
if (!check.nrml.epsilonEquals(cpair.nrml, TOL)) return false;
} else {
final Vector3d del = new Vector3d();
del.sub(cpair.pnt2, cpair.pnt1);
if (Math.abs(check.dist - cpair.dist) > TOL || Math.abs(del.length() - check.dist) > TOL)
return false;
if (!cpair.pnt1.epsilonEquals(check.pnt1, TOL)) return false;
if (!cpair.pnt2.epsilonEquals(check.pnt2, TOL)) return false;
if (cpair.feat1.getType() != check.feat1.getType()
|| cpair.feat2.getType() != check.feat2.getType()) return false;
}
if (check.dist > 0) if (!cpair.nrml.epsilonEquals(check.nrml, TOL)) return false;
return true;
}
/**
* 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);
}
}
/**
* 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;
}
/**
* Returns the normal of the plane closest to the given origin.
*
* @param pointOnAABB A point on the AABB
* @param origin The origin
* @param testX True if the x-axis should be tested
* @param testY True if the y-axis should be tested
* @param testZ True if the z-axis should be tested
* @return The normal
*/
public Vector3d normalForPlaneClosestToOrigin(
Vector3d pointOnAABB, Vector3d origin, boolean testX, boolean testY, boolean testZ) {
ArrayList<Vector3d> normals = new ArrayList<Vector3d>();
if (pointOnAABB.z == minZ() && testZ) normals.add(new Vector3d(0, 0, -1));
if (pointOnAABB.z == maxZ() && testZ) normals.add(new Vector3d(0, 0, 1));
if (pointOnAABB.x == minX() && testX) normals.add(new Vector3d(-1, 0, 0));
if (pointOnAABB.x == maxX() && testX) normals.add(new Vector3d(1, 0, 0));
if (pointOnAABB.y == minY() && testY) normals.add(new Vector3d(0, -1, 0));
if (pointOnAABB.y == maxY() && testY) normals.add(new Vector3d(0, 1, 0));
double minDistance = Double.MAX_VALUE;
Vector3d closestNormal = new Vector3d();
for (int i = 0; i < normals.size(); i++) {
Vector3d n = normals.get(i);
Vector3d diff = new Vector3d(centerPointForNormal(n));
diff.sub(origin);
double distance = diff.length();
if (distance < minDistance) {
minDistance = distance;
closestNormal = n;
}
}
return closestNormal;
}
//
// places triangle into canonical position
//
private void add(double len01, Vector3d v0, Vector3d v1, Vector3d v2) {
// area of triangle
p1.set(v1);
p2.set(v2);
p1.sub(v0);
p2.sub(v0);
normal.cross(p1, p2);
// tri area
double triWidth = len01;
double triArea2 = normal.length();
double triHeight = triArea2 / triWidth;
double v1x = triWidth;
double v2y = triHeight; // y- coordinate of 2D triangle 2
double v2x = p1.dot(p2) / len01;
m_ctri.add(new CanonicalTri(v1x / m_pixelSize, v2x / m_pixelSize, v2y / m_pixelSize, m_gap));
//
// all triangles are oriented canonically: v0,v1 is the longest side
//
m_tri.add(new Vector3d(v0));
m_tri.add(new Vector3d(v1));
m_tri.add(new Vector3d(v2));
m_triCount++;
}
// 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;
}
// gets a point not on vector a...b; this can be used to define a plan or cross products
private static Vector3d getNonColinearVector(Vector3d ab) {
Vector3d cr = new Vector3d();
cr.cross(ab, XV);
if (cr.length() > 0.00001) {
return XV;
} else {
return YV;
}
}
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;
}
public Vector3d getReaction(Vector3d position, Vector3d velocity, Vector3d action, double mass) {
Vector3d relativePosition = new Vector3d(position);
relativePosition.sub(point);
double x = relativePosition.length();
Vector3d reaction = new Vector3d(relativePosition);
if (x > Teal.DoubleZero) {
reaction.scale(-k1);
}
double v = velocity.length();
Vector3d damping = new Vector3d(velocity);
if (v > Teal.DoubleZero) {
// damping.scale(1./v);
damping.scale(-k2 * (1.0 + 0.0 / v) / (1. + (x / p) * (x / p)));
reaction.add(damping);
}
lastReaction.set(reaction);
return reaction;
}
public Transform3D getTransform() {
Transform3D transform = new Transform3D();
Point3d target = new Point3d();
target.add(direction, position);
Point3d zoomedPosition = new Point3d();
zoomedPosition.scaleAdd(-zoom / direction.length(), direction, target);
transform.lookAt(zoomedPosition, target, up);
transform.invert();
return transform;
}
private boolean projectToPlane(Vector3d projVec, Vector3d planeVec) {
double dis = planeVec.dot(projVec);
projVec.x = projVec.x - planeVec.x * dis;
projVec.y = projVec.y - planeVec.y * dis;
projVec.z = projVec.z - planeVec.z * dis;
double length = projVec.length();
if (length < EPSILON) { // projVec is parallel to planeVec
return false;
}
projVec.scale(1 / length);
return true;
}
protected void computeCircle() {
Vector3d center = new Vector3d((Vector3d) get(0));
Vector3d p2 = new Vector3d((Vector3d) get(1));
p2.sub(center);
double radius = p2.length();
removeAllElements();
double angle = 0;
double incAngle = 2 * Math.PI / sides;
for (int i = 0; i < sides; i++) {
super.add(
new Vector3d(
center.x + radius * Math.cos(angle), center.y + radius * Math.sin(angle), 0));
angle += incAngle;
}
super.add(new Vector3d(center.x + radius, center.y, 0));
}
/**
* 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);
}
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);
}
/**
* 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;
}
@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");
}
}
public static void setRotation(Quat4d q, Vector3d axis, double angle) {
double d = axis.length();
assert (d != 0f);
double s = (double) Math.sin(angle * 0.5f) / d;
q.set(axis.x * s, axis.y * s, axis.z * s, (double) Math.cos(angle * 0.5f));
}
// 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);
}
}