public double orthogonalDistance(Point3D point) { temporaryVector.set(point); temporaryVector.sub(this.point); return temporaryVector.dot(normal); }
public boolean isInVoronoiRegionOfVertex(Point3D pointToCheck, Point3D otherPoint1, Point3D otherPoint2) { tempVector1.set(pointToCheck); tempVector1.scale(-1.0); tempVector2.sub(otherPoint1, pointToCheck); if (!(tempVector1.dot(tempVector2) <= 0.0)) return false; tempVector2.sub(otherPoint2, pointToCheck); if (!(tempVector1.dot(tempVector2) <= 0.0)) return false; return true; }
/** {@inheritDoc} */ @Override public void setJointOrientation(Orientation3DReadOnly jointOrientation) { jointOrientation.getRotationVector(rotationVector); setQ(rotationVector.dot(jointAxis)); }
public boolean isInVoronoiRegionOfVertex(Point3D pointToCheck, Point3D otherPoint1, Point3D otherPoint2, Point3D otherPoint3) { tempVector1.set(pointToCheck); tempVector1.scale(-1.0); tempVector2.sub(otherPoint1, pointToCheck); double dotProduct = tempVector1.dot(tempVector2); if (!(dotProduct <= 0.0)) return false; tempVector2.sub(otherPoint2, pointToCheck); dotProduct = tempVector1.dot(tempVector2); if (!(dotProduct <= 0.0)) return false; tempVector2.sub(otherPoint3, pointToCheck); dotProduct = tempVector1.dot(tempVector2); if (!(dotProduct <= 0.0)) return false; return true; }
public Color getNormalBasedColor(Vector3D normal, boolean isNormalSet) { Color color = DEFAULT_COLOR; if (isNormalSet) { Vector3D zUp = new Vector3D(0.0, 0.0, 1.0); normal.normalize(); double angle = Math.abs(zUp.dot(normal)); double hue = 120.0 * angle; color = Color.hsb(hue, 1.0, 1.0); } return color; }
public Color getNormalBasedColor(Vector3D normal, boolean isNormalSet) { Color color = DEFAULT_COLOR; if (isNormalSet) { Vector3D zUp = new Vector3D(0.0, 0.0, 1.0); normal.normalize(); double angle = Math.abs(zUp.dot(normal)); double hue = 120.0 * angle; color = Color.hsb(hue, 1.0, 1.0); } return color; }
public boolean isInVoronoiRegionOfVertex(Point3D pointToCheck, Point3D otherPoint) { tempVector1.set(pointToCheck); tempVector1.scale(-1.0); tempVector2.sub(otherPoint, pointToCheck); return (tempVector1.dot(tempVector2) <= 0.0); }
public boolean isInVoronoiRegionOfFace(Point3D facePointOne, Point3D facePointTwo, Point3D facePointThree, Point3D otherPoint) { tempVector1.sub(facePointTwo, facePointOne); tempVector2.sub(facePointThree, facePointOne); tempNormalVector1.cross(tempVector1, tempVector2); tempVector1.set(facePointOne); tempVector1.scale(-1.0); double dot1 = tempVector1.dot(tempNormalVector1); tempVector3.sub(otherPoint, facePointOne); double dot2 = tempVector3.dot(tempNormalVector1); // TODO: Magic delta here. Figure out robustness to deltas. if (Math.abs(dot2) < 1e-10)//1e-6) return true; // Other point is in the same plane. Just project to the plane then. return (dot1 * dot2 < 0.0); // return (dot1 * dot2 < 0.0 + 1e-8); }
public static double getHeading(ReferenceFrame ankleFrame) { RigidBodyTransform transform = ankleFrame.getTransformToDesiredFrame(worldFrame); Vector3D xVector = new Vector3D(1.0, 0.0, 0.0); transform.transform(xVector); xVector.sub(new Vector3D(0.0, 0.0, -xVector.getZ())); xVector.normalize(); if (Double.isNaN(xVector.getX())) { throw new RuntimeException("FootstepUtils: Footsteps cannot face directly upwards"); } double cosTheta = xVector.dot(new Vector3D(1.0, 0.0, 0.0)); double sinTheta = xVector.dot(new Vector3D(0.0, 1.0, 0.0)); double theta = Math.atan2(sinTheta, cosTheta); return theta; } }
private void projectOriginOntoEdge(Point3D vertexOne, Point3D vertexTwo, Point3D projectionToPack) { tempVector1.set(vertexOne); tempVector1.scale(-1.0); tempVector2.sub(vertexTwo, vertexOne); double percentFromVertexOneToVertexTwo = tempVector1.dot(tempVector2) / (tempVector2.dot(tempVector2)); tempVector2.scale(percentFromVertexOneToVertexTwo); projectionToPack.set(vertexOne); projectionToPack.add(tempVector2); double oneMinusPercentFromVertexOneToVertexTwo = 1.0 - percentFromVertexOneToVertexTwo; lambdas.clear(); setLambda(vertexOne, oneMinusPercentFromVertexOneToVertexTwo); setLambda(vertexTwo, percentFromVertexOneToVertexTwo); }
public double orhtogonalDistance(NormalOcTreeNode node) { temporaryVector.set(node.getHitLocationX(), node.getHitLocationY(), node.getHitLocationZ()); temporaryVector.sub(point); return temporaryVector.dot(normal); }
private void parallelAxisTheorem(Matrix3D inputInertia, double mass, Vector3D vector1, Matrix3D outputInertia) { outputInertia.set(inputInertia); double dotProduct = vector1.dot(vector1); for (int i=0; i<3; i++) { for (int j=0; j<3; j++) { double elementValueAdjustment = 0.0; if (i == j) elementValueAdjustment = dotProduct; elementValueAdjustment = elementValueAdjustment - getElement(i, vector1) * getElement(j, vector1); elementValueAdjustment = elementValueAdjustment * mass; double elementValue = outputInertia.getElement(i, j); elementValue = elementValue + elementValueAdjustment; outputInertia.setElement(i, j, elementValue); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNextOrthogonalVector3D() { Random random = new Random(4876L); for (int i = 0; i < 100; i++) { double length = RandomNumbers.nextDouble(random, 0.1, 100.0); Vector3D vector = RandomGeometry.nextVector3D(random, length); Vector3D orthoVector = RandomGeometry.nextOrthogonalVector3D(random, vector, true); assertEquals(0.0, vector.dot(orthoVector), 1.0e-12); assertEquals(1.0, orthoVector.length(), 1.0e-12); } } }
@Test public void testNormalizeDirection() throws Exception { Random random = new Random(3242L); for (int i = 0; i < ITERATIONS; i++) { Line3D line = new Line3D(); Point3D point = EuclidCoreRandomTools.nextPoint3D(random); Vector3D direction = EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0); line.set(point, direction); assertFalse(direction.epsilonEquals(line.getDirection(), 1.0e-3)); assertTrue(direction.dot(line.getDirection()) > 0.0); assertEquals(direction.length(), direction.dot(line.getDirection()), EPSILON); assertEquals(1.0, line.getDirection().length(), EPSILON); } }
private void updateNormalAndOriginOnly(NormalOcTreeNode node) { node.getNormal(temporaryVector); // TODO Review and possibly improve dealing with normal flips. if (getNumberOfNodes() >= 1 && temporaryVector.dot(normal) < 0.0) temporaryVector.negate(); normal.update(temporaryVector); point.update(node.getHitLocationX(), node.getHitLocationY(), node.getHitLocationZ()); }
public double computeTripleProductIfTetragon() { if (pointFour == null) return Double.NaN; Vector3D vectorAB = new Vector3D(); Vector3D vectorAC = new Vector3D(); Vector3D vectorAD = new Vector3D(); Vector3D normalVector = new Vector3D(); vectorAB.sub(pointTwo, pointOne); vectorAC.sub(pointThree, pointOne); vectorAD.sub(pointFour, pointOne); normalVector.cross(vectorAB, vectorAC); double tripleProduct = vectorAD.dot(normalVector); return tripleProduct; }
private static double computeScalarInertiaAroundJointAxis(Link link, PinJoint pinJoint) { Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); Vector3D jointAxis = new Vector3D(); pinJoint.getJointAxis(jointAxis); Vector3D temp1 = new Vector3D(jointAxis); momentOfInertia.transform(temp1); double scalarInertiaAboutCoM = jointAxis.dot(temp1); // jointAxis^T * momentOfInertia * jointAxis double mass = link.getMass(); Vector3D offsetToCoM = new Vector3D(link.getComOffset()); Vector3D offset = new Vector3D(); pinJoint.getOffset(offset); offsetToCoM.sub(offset); // c - p Vector3D temp3 = new Vector3D(jointAxis); temp3.scale(offsetToCoM.dot(jointAxis)); // ((c - p) . a) * a Vector3D comToJointAxis = new Vector3D(); comToJointAxis.sub(offsetToCoM, temp3); // (c - p) - ((c - p) . a) * a double distanceToJointAxis = comToJointAxis.length(); double scalarInertiaAboutJointAxis = scalarInertiaAboutCoM + mass * distanceToJointAxis * distanceToJointAxis; return scalarInertiaAboutJointAxis; }
public void recomputeNormalAndOrigin() { pca.clear(); nodes.stream().forEach(node -> pca.addPoint(node.getHitLocationX(), node.getHitLocationY(), node.getHitLocationZ())); pca.compute(); Point3D mean = new Point3D(); pca.getMean(mean); point.clear(); point.update(mean, getNumberOfNodes()); Vector3D thirdVector = new Vector3D(); pca.getThirdVector(thirdVector); pca.getStandardDeviation(standardDeviationPrincipalValues); if (thirdVector.dot(normal) < 0.0) thirdVector.negate(); normal.clear(); normal.update(thirdVector, getNumberOfNodes()); }
@Override public void updateLeaf(NormalOcTreeNode leafToUpdate, OcTreeKeyReadOnly leafKey, boolean nodeJustCreated) { OccupancyTools.updateNodeLogOdds(parameters, leafToUpdate, updateLogOdds); leafToUpdate.updateHitLocation(hitLocation, updateWeight, maximumNumberOfHits); if (!leafToUpdate.isNormalSet()) { initialGuessNormal.sub(sensorLocation, hitLocation); initialGuessNormal.normalize(); leafToUpdate.setNormal(initialGuessNormal); leafToUpdate.setNormalQuality(Float.POSITIVE_INFINITY, 0); } else // TODO review normal flips. { initialGuessNormal.sub(sensorLocation, hitLocation); leafToUpdate.getNormal(nodeNormal); if (nodeNormal.dot(initialGuessNormal) < 0.0) leafToUpdate.negateNormal(); } }