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); } }