Java Code Examples for javax.vecmath.Vector3d#normalize()
The following examples show how to use
javax.vecmath.Vector3d#normalize() .
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example 1
Source File: Spidee.java From Robot-Overlord-App with GNU General Public License v2.0 | 6 votes |
void Translate_Body_Towards(Vector3d point,float dt) { Vector3d dp = new Vector3d(point); dp.sub( body.pos ); double dpl = dp.length(); if( dpl > dt ) { dp.normalize(); dp.scale( dt ); if( body.pos.z != 0 || dp.z >= 0 ) { body.pos.add(dp); int i; for( i = 0; i < 6; ++i ) { legs[i].pan_joint.pos.add( dp ); } } } else { body.pos = point; } }
Example 2
Source File: EarthVector.java From geowave with Apache License 2.0 | 6 votes |
/** Vector3d (unit vector)/elev constructor */ public EarthVector(final Vector3d vec, final double inelev) { final Vector3d norm = vec; norm.normalize(); final double sinlat = norm.z; final double coslat = Math.sqrt(Math.abs(1.0 - (sinlat * sinlat))); latitude = Math.atan2(sinlat, coslat); final double cosa = norm.x / coslat; final double sina = norm.y / coslat; double vra; if (Math.abs(cosa) < 0.001) { vra = RAD_90 * sign(sina); } else { vra = Math.atan2(sina, cosa); } longitude = vra; elevation = inelev; initVector(); }
Example 3
Source File: Spidee.java From Robot-Overlord-App with GNU General Public License v2.0 | 5 votes |
void Stand_Up(double dt) { int i; int onfloor = 0; float scale = 2.0f; // touch the feet to the floor for(i=0;i<6;++i) { if(legs[i].ankle_joint.pos.z>0) legs[i].ankle_joint.pos.z-=4*scale*dt; else ++onfloor; // contract - put feet closer to shoulders Vector3d df = new Vector3d(legs[i].ankle_joint.pos); df.sub(body.pos); df.z=0; if(df.length()>standing_radius) { df.normalize(); df.scale(6*scale*dt); legs[i].ankle_joint.pos.sub(df); } } if(onfloor==6) { // we've planted all feet, raise the body a bit if( body.pos.z < standing_height ) body.pos.z+=2*scale*dt; for(i=0;i<6;++i) { Vector3d ds = new Vector3d( legs[i].pan_joint.pos ); ds.sub( body.pos ); ds.normalize(); ds.scale(standing_radius); legs[i].npoc.set(body.pos.x+ds.x, body.pos.y+ds.y, 0); } } }
Example 4
Source File: QuatSymmetrySubunits.java From biojava with GNU Lesser General Public License v2.1 | 5 votes |
private void calcCenters() { for (Point3d p : originalCenters) { Point3d c = new Point3d(p); c.sub(centroid); centers.add(c); Vector3d v = new Vector3d(c); v.normalize(); unitVectors.add(v); } }
Example 5
Source File: EarthVector.java From geowave with Apache License 2.0 | 5 votes |
private double internalGetAzimuth(final EarthVector loc) { // Calculate the True North unit vector final EarthVector locNorth = new EarthVector(this); final double radInc = Math.max(RAD_1, Math.abs(loc.getLatitude() - getLatitude())); final boolean calcNorth = (latitude < loc.getLatitude()); if (calcNorth) { locNorth.setLatitude(locNorth.getLatitude() + radInc); } else { locNorth.setLatitude(locNorth.getLatitude() - radInc); } final Vector3d vecNorth = locNorth.getVector(); vecNorth.sub(ecfVector); // Calculate the azimuth between this and loc final Vector3d vecTemp = new Vector3d(loc.getVector()); vecTemp.sub(ecfVector); vecNorth.normalize(); vecTemp.normalize(); double azimuth = Math.acos(vecNorth.dot(vecTemp)); if (!calcNorth) { azimuth = RAD_180 - azimuth; } final double deltaLon = Math.abs(loc.getLongitude() - longitude); if (((loc.getLongitude() < longitude) && (deltaLon < RAD_180)) || ((loc.getLongitude() > longitude) && (deltaLon > RAD_180))) { // normalize azimuth to 0-360 degrees azimuth = RAD_360 - azimuth; } return azimuth; }
Example 6
Source File: EarthVector.java From geowave with Apache License 2.0 | 5 votes |
/** * Rotates this coordinate about the input vector through the input angle (radians - because we * usually use this internally) * * @param rotAxis The axis of rotation * @param angle The angle of rotation (in radians) */ public Vector3d rotate(final Vector3d rotAxis, final double angle) { final Vector3d thisVec = new Vector3d(ecfVector); final Vector3d axis = new Vector3d(rotAxis); axis.normalize(); final Matrix3d trans = new Matrix3d(); trans.set(new AxisAngle4d(axis, angle)); trans.transform(thisVec); return thisVec; }
Example 7
Source File: EarthVector.java From geowave with Apache License 2.0 | 5 votes |
public EarthVector findPointReverseDirection(final EarthVector nextCoord, final double fraction) { // check for same point first if (equals(nextCoord)) { return new EarthVector(this); } // compute the vector normal to this vector and the input vector final Vector3d nextVector = nextCoord.getVector(); final Vector3d vec = new Vector3d(); vec.cross(ecfVector, nextVector); vec.negate(); // compute the fractional angle between this vector and the input vector final double phi = fraction * Math.acos(ecfVector.dot(nextVector) / (ecfVector.length() * nextVector.length())); // create the vector rotated through phi about the normal vector final Vector3d output = rotate(vec, phi); // now scale the output vector by interpolating the magnitudes // of this vector and the input vector output.normalize(); final double size = ((nextVector.length() - ecfVector.length()) * fraction) + ecfVector.length(); output.scale(size); return new EarthVector(output); }
Example 8
Source File: EarthVector.java From geowave with Apache License 2.0 | 5 votes |
/** * Locate a coordinate on the line between this one and the "next" coord, at some fraction of the * distance between them */ public EarthVector findPoint(final EarthVector nextCoord, final double fraction) { // check for same point first if (equals(nextCoord)) { return new EarthVector(this); } // compute the vector normal to this vector and the input vector final Vector3d nextVector = nextCoord.getVector(); final Vector3d vec = new Vector3d(); vec.cross(ecfVector, nextVector); // compute the fractional angle between this vector and the input vector final double phi = fraction * Math.acos(ecfVector.dot(nextVector) / (ecfVector.length() * nextVector.length())); // create the vector rotated through phi about the normal vector final Vector3d output = rotate(vec, phi); // now scale the output vector by interpolating the magnitudes // of this vector and the input vector output.normalize(); final double size = ((nextVector.length() - ecfVector.length()) * fraction) + ecfVector.length(); output.scale(size); return new EarthVector(output); }
Example 9
Source File: CavesExporter.java From WorldPainter with GNU General Public License v3.0 | 5 votes |
private void createTunnel(MinecraftWorld world, Dimension dimension, Random random, CaveSettings tunnelSettings, boolean surfaceBreaking, int minimumLevel) { Point3d location = new Point3d(tunnelSettings.start.x, tunnelSettings.start.y, tunnelSettings.start.z); Vector3d direction = getRandomDirection(random); final double minRadius = tunnelSettings.minRadius, maxRadius = tunnelSettings.maxRadius, radiusChangeSpeed = tunnelSettings.radiusChangeSpeed; double length = 0.0, radius = (maxRadius + minRadius) / 2.0, radiusDelta = 0.0; final int maxLength = tunnelSettings.length, twistiness = tunnelSettings.twistiness; if (logger.isTraceEnabled()) { logger.trace("Creating tunnel @ {},{},{} of length {}; radius: {} - {} (variability: {}); twistiness: {}", tunnelSettings.start.x, tunnelSettings.start.y, tunnelSettings.start.z, maxLength, tunnelSettings.minRadius, tunnelSettings.maxRadius, radiusChangeSpeed, twistiness); } while (length < maxLength) { if ((minimumLevel == 0) && (dimension.getLayerValueAt(Caves.INSTANCE, (int) location.x, (int) location.y) < 1)) { // Don't stray into areas where the layer isn't present at all return; } excavate(world, dimension, random, tunnelSettings, location, radius, surfaceBreaking); length += direction.length(); location.add(direction); final Vector3d dirChange = getRandomDirection(random); dirChange.scale(random.nextDouble() / (5 - twistiness)); direction.add(dirChange); direction.normalize(); if (radiusChangeSpeed > 0.0) { radius = MathUtils.clamp(minRadius, radius + radiusDelta, maxRadius); radiusDelta += random.nextDouble() * 2 * radiusChangeSpeed - radiusChangeSpeed; } } }
Example 10
Source File: EarthVector.java From geowave with Apache License 2.0 | 5 votes |
/** * Vector3d (ECF or unit vector) constructor If vector is ECF, elevation is derived from it * Otherwise, elevation is zero */ public EarthVector(final Vector3d vec) { final Vector3d norm = new Vector3d(vec); norm.normalize(); final double sinlat = norm.z; final double coslat = Math.sqrt(Math.abs(1.0 - (sinlat * sinlat))); latitude = Math.atan2(sinlat, coslat); double vra; // not sure which epsilon value is most appropriate - just chose Y eps. // because it's bigger if (Math.abs(coslat) <= Y_EPSILON) { // this value's kind of arbitrary in this case vra = 0.0; } else { // this unchecked divide by 0 was causing EV's to have NaN's and // such // sometimes, causing stuff to break, especially for the globe view final double cosa = norm.x / coslat; final double sina = norm.y / coslat; if (Math.abs(cosa) < X_EPSILON) { vra = RAD_90 * sign(sina); } else { vra = Math.atan2(sina, cosa); } } longitude = vra; if (vec.length() > getEarthRadiusKM()) { elevation = vec.length() - getEarthRadiusKM(); } else { elevation = 0.0; } initVector(); }
Example 11
Source File: Spidee.java From Robot-Overlord-App with GNU General Public License v2.0 | 5 votes |
boolean Sit_Down(double dt) { int i; int legup=0; float scale=1.0f; // we've planted all feet, lower the body to the ground if( body.pos.z > 0 ) body.pos.z -= 2 * scale * dt; else { for( i = 0; i < 6; ++i ) { // raise feet Vector3d ls = new Vector3d( legs[i].ankle_joint.pos ); ls.sub( legs[i].pan_joint.pos ); if( ls.length() < 16 ) { ls.z=0; ls.normalize(); ls.scale( 4 * scale * dt ); legs[i].ankle_joint.pos.add( ls ); } else ++legup; if( legs[i].ankle_joint.pos.z-legs[i].pan_joint.pos.z < 5.5 ) legs[i].ankle_joint.pos.z += 4 * scale * dt; else ++legup; if( legs[i].knee_joint.pos.z-legs[i].pan_joint.pos.z < 5.5 ) legs[i].knee_joint.pos.z += 4 * scale * dt; else ++legup; } if( legup == 6*3 ) return true; } return false; }
Example 12
Source File: DetectorProperties.java From dawnsci with Eclipse Public License 1.0 | 4 votes |
/** * @return scattering angle (two-theta) associated with pixel */ public double pixelScatteringAngle(final double x, final double y) { Vector3d p = pixelPosition(x, y); p.normalize(); return Math.acos(p.dot(beamVector)); }
Example 13
Source File: DetectorProperties.java From dawnsci with Eclipse Public License 1.0 | 4 votes |
/** * @param beamVector * The beam vector to set. */ public void setBeamVector(Vector3d beamVector) { this.beamVector = beamVector; beamVector.normalize(); }
Example 14
Source File: EarthVector.java From geowave with Apache License 2.0 | 4 votes |
/** * Locate a coordinate at a specific distance (km), elevation angle (radians), and heading * (radians) from this one. */ public EarthVector findPoint( final double distanceKM, final double azimuth, final double elevAngle) { // convert distance to radians // final double distR = distanceKM / KMPerDegree() / DPR; final double lon = getLongitude(); final double lat = getLatitude(); // convert local enu to ecf to get east and north vectors // east vector final Vector3d eastVec = new Vector3d(1, 0, 0); final Vector3d northVec = new Vector3d(0, 1, 0); final double sinLon = Math.sin(lon); final double cosLon = Math.cos(lon); final double sinLat = Math.sin(lat); final double cosLat = Math.cos(lat); final Matrix3d enuToEcf = new Matrix3d(); enuToEcf.m00 = -sinLon; enuToEcf.m01 = -(sinLat * cosLon); enuToEcf.m02 = cosLat * cosLon; enuToEcf.m10 = cosLon; enuToEcf.m11 = -(sinLat * sinLon); enuToEcf.m12 = cosLat * sinLon; enuToEcf.m20 = 0; enuToEcf.m21 = cosLat; enuToEcf.m22 = sinLat; enuToEcf.transform(eastVec); enuToEcf.transform(northVec); eastVec.normalize(); northVec.normalize(); northVec.scale(distanceKM); final Matrix3d elevTrans = new Matrix3d(); elevTrans.set(new AxisAngle4d(eastVec, elevAngle)); elevTrans.transform(northVec); final Matrix3d azTrans = new Matrix3d(); final Vector3d unitEcf = new Vector3d(ecfVector); unitEcf.normalize(); azTrans.set(new AxisAngle4d(unitEcf, azimuth)); azTrans.transform(northVec); final Vector3d transformedEcf = new Vector3d(); transformedEcf.add(ecfVector, northVec); final EarthVector transformedEv = new EarthVector(transformedEcf); return transformedEv; }
Example 15
Source File: DragBallEntity.java From Robot-Overlord-App with GNU General Public License v2.0 | 4 votes |
public void renderTranslation(GL2 gl2) { // camera forward is -z axis RobotOverlord ro = (RobotOverlord)getRoot(); PoseEntity camera = ro.viewport.getAttachedTo(); Vector3d lookAtVector = subject.getPosition(); lookAtVector.sub(camera.getPosition()); lookAtVector.normalize(); float r = (majorAxis==Axis.X) ? 1 : 0.5f; float g = (majorAxis==Axis.Y) ? 1 : 0.5f; float b = (majorAxis==Axis.Z) ? 1 : 0.5f; Vector3d fx = MatrixHelper.getXAxis(FOR); Vector3d fy = MatrixHelper.getYAxis(FOR); Vector3d fz = MatrixHelper.getZAxis(FOR); // should we hide an axis if it points almost the same direction as the camera? boolean drawX = (Math.abs(lookAtVector.dot(fx))<0.95); boolean drawY = (Math.abs(lookAtVector.dot(fy))<0.95); boolean drawZ = (Math.abs(lookAtVector.dot(fz))<0.95); if(drawX) { gl2.glColor3f(r,0,0); renderTranslationHandle(gl2,new Vector3d(1,0,0)); } if(drawY) { gl2.glColor3f(0,g,0); renderTranslationHandle(gl2,new Vector3d(0,1,0)); } if(drawZ) { gl2.glColor3f(0,0,b); renderTranslationHandle(gl2,new Vector3d(0,0,1)); } if(isActivelyMoving) { // the distance moved. gl2.glBegin(GL2.GL_LINES); gl2.glColor3f(255,255,255); gl2.glVertex3d(0,0,0); gl2.glVertex3d( (startMatrix.m03-resultMatrix.m03)/ballSize.get(), (startMatrix.m13-resultMatrix.m13)/ballSize.get(), (startMatrix.m23-resultMatrix.m23)/ballSize.get()); gl2.glEnd(); } }
Example 16
Source File: Spidee.java From Robot-Overlord-App with GNU General Public License v2.0 | 4 votes |
void Move_Calculate_Angles() { int i,j; double x,y; for(i=0;i<6;++i) { SpideeLeg leg=legs[i]; // find the pan angle Vector3d sf= new Vector3d( leg.pan_joint.pos ); sf.sub( body.pos ); sf.normalize(); Vector3d sl=new Vector3d(); sl.cross( body.up, sf ); x = leg.pan_joint.forward.dot(sf); y = leg.pan_joint.forward.dot(sl); double pan_angle = Math.atan2( y, x ); // find the tilt angle x = leg.tilt_joint.forward.dot(leg.pan_joint.forward); y = leg.tilt_joint.forward.dot(leg.pan_joint.up ); double tilt_angle = Math.atan2( y, x ); // find the knee angle x = leg.knee_joint.forward.dot(leg.tilt_joint.forward); y = leg.knee_joint.forward.dot(leg.tilt_joint.up ); double knee_angle = Math.atan2( y, x ); // translate the angles into the servo range, 0...255 over 0...PI. final double scale = ( 255.0f / Math.PI ); if( i < 3 ) pan_angle = -pan_angle; double p = leg.pan_joint .zero - pan_angle * leg.pan_joint .scale * scale; double t = leg.tilt_joint.zero + tilt_angle * leg.tilt_joint.scale * scale; double k = leg.knee_joint.zero - knee_angle * leg.knee_joint.scale * scale; leg.pan_joint .angle = p; leg.tilt_joint.angle = t; leg.knee_joint.angle = k; // record the history for the graphs for( j = 0; j < SpideeJoint.ANGLE_HISTORY_LENGTH-1; ++j ) { leg.pan_joint .angle_history[j] = leg.pan_joint .angle_history[j+1]; leg.tilt_joint.angle_history[j] = leg.tilt_joint.angle_history[j+1]; leg.knee_joint.angle_history[j] = leg.knee_joint.angle_history[j+1]; } //memcpy( leg.pan_joint .angle_history, leg.pan_joint .angle_history + sizeof(float), ( Joint.ANGLE_HISTORY_LENGTH - 1 ) * sizeof(float) ); //memcpy( leg.tilt_joint.angle_history, leg.tilt_joint.angle_history + sizeof(float), ( Joint.ANGLE_HISTORY_LENGTH - 1 ) * sizeof(float) ); //memcpy( leg.knee_joint.angle_history, leg.knee_joint.angle_history + sizeof(float), ( Joint.ANGLE_HISTORY_LENGTH - 1 ) * sizeof(float) ); leg.pan_joint .angle_history[ SpideeJoint.ANGLE_HISTORY_LENGTH - 1 ] = p - leg.pan_joint .zero; leg.tilt_joint.angle_history[ SpideeJoint.ANGLE_HISTORY_LENGTH - 1 ] = t - leg.tilt_joint.zero; leg.knee_joint.angle_history[ SpideeJoint.ANGLE_HISTORY_LENGTH - 1 ] = k - leg.knee_joint.zero; // @TODO: contrain angles in the model to the limits set in joint::angle_max and joint::angle_min } }
Example 17
Source File: SystematicSolver.java From biojava with GNU Lesser General Public License v2.1 | 4 votes |
private boolean evaluatePermutation(List<Integer> permutation) { // permutate subunits for (int j = 0, n = subunits.getSubunitCount(); j < n; j++) { transformedCoords[j].set(originalCoords[permutation.get(j)]); } int fold = PermutationGroup.getOrder(permutation); // TODO implement this piece of code using at origin superposition Quat4d quat = UnitQuaternions.relativeOrientation( originalCoords, transformedCoords); AxisAngle4d axisAngle = new AxisAngle4d(); Matrix4d transformation = new Matrix4d(); transformation.set(quat); axisAngle.set(quat); Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z); if (axis.lengthSquared() < 1.0E-6) { axisAngle.x = 0; axisAngle.y = 0; axisAngle.z = 1; axisAngle.angle = 0; } else { axis.normalize(); axisAngle.x = axis.x; axisAngle.y = axis.y; axisAngle.z = axis.z; } CalcPoint.transform(transformation, transformedCoords); double subunitRmsd = CalcPoint.rmsd(transformedCoords, originalCoords); if (subunitRmsd <parameters.getRmsdThreshold()) { // transform to original coordinate system combineWithTranslation(transformation); QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation); if (scores.getRmsd() < 0.0 || scores.getRmsd() > parameters.getRmsdThreshold()) { return false; } scores.setRmsdCenters(subunitRmsd); Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores); rotations.addRotation(symmetryOperation); return true; } return false; }
Example 18
Source File: C2RotationSolver.java From biojava with GNU Lesser General Public License v2.1 | 4 votes |
private void solve() { initialize(); Vector3d trans = new Vector3d(subunits.getCentroid()); trans.negate(); List<Point3d[]> traces = subunits.getTraces(); // Point3d[] x = SuperPosition.clonePoint3dArray(traces.get(0)); // SuperPosition.center(x); // Point3d[] y = SuperPosition.clonePoint3dArray(traces.get(1)); // SuperPosition.center(y); Point3d[] x = CalcPoint.clonePoint3dArray(traces.get(0)); CalcPoint.translate(trans, x); Point3d[] y = CalcPoint.clonePoint3dArray(traces.get(1)); CalcPoint.translate(trans, y); // TODO implement this piece of code using at origin superposition Quat4d quat = UnitQuaternions.relativeOrientation( x, y); AxisAngle4d axisAngle = new AxisAngle4d(); Matrix4d transformation = new Matrix4d(); transformation.set(quat); axisAngle.set(quat); Vector3d axis = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z); if (axis.lengthSquared() < 1.0E-6) { axisAngle.x = 0; axisAngle.y = 0; axisAngle.z = 1; axisAngle.angle = 0; } else { axis.normalize(); axisAngle.x = axis.x; axisAngle.y = axis.y; axisAngle.z = axis.z; } CalcPoint.transform(transformation, y); // if rmsd or angle deviation is above threshold, stop double angleThresholdRadians = Math.toRadians(parameters.getAngleThreshold()); double deltaAngle = Math.abs(Math.PI-axisAngle.angle); if (deltaAngle > angleThresholdRadians) { rotations.setC1(subunits.getSubunitCount()); return; } // add unit operation addEOperation(); // add C2 operation int fold = 2; combineWithTranslation(transformation); List<Integer> permutation = Arrays.asList(1,0); QuatSymmetryScores scores = QuatSuperpositionScorer.calcScores(subunits, transformation, permutation); scores.setRmsdCenters(0.0); // rmsd for superposition of two subunits centers is zero by definition if (scores.getRmsd() > parameters.getRmsdThreshold() || deltaAngle > angleThresholdRadians) { rotations.setC1(subunits.getSubunitCount()); return; } Rotation symmetryOperation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores); rotations.addRotation(symmetryOperation); }
Example 19
Source File: DynamicObject.java From jMAVSim with BSD 3-Clause "New" or "Revised" License | 4 votes |
@Override public void update(long t) { if (lastTime >= 0) { double dt = (t - lastTime) / 1000.0; if (dt < 0.001) { dt = 0.001; // Limit min dt to 1ms } // Position Vector3d dPos = new Vector3d(velocity); dPos.scale(dt); position.add(dPos); // Velocity acceleration = getForce(); acceleration.scale(1.0 / mass); acceleration.add(getWorld().getEnvironment().getG()); if (position.z >= getWorld().getEnvironment().getGroundLevel(position) && velocity.z + acceleration.z * dt >= 0.0) { // On ground acceleration.x = -velocity.x / dt; acceleration.y = -velocity.y / dt; acceleration.z = -velocity.z / dt; position.z = getWorld().getEnvironment().getGroundLevel(position); //rotationRate.set(0.0, 0.0, 0.0); } Vector3d dVel = new Vector3d(acceleration); dVel.scale(dt); velocity.add(dVel); // Rotation if (rotationRate.length() > 0.0) { Matrix3d r = new Matrix3d(); Vector3d rotationAxis = new Vector3d(rotationRate); rotationAxis.normalize(); r.set(new AxisAngle4d(rotationAxis, rotationRate.length() * dt)); rotation.mulNormalize(r); } // Rotation rate Vector3d Iw = new Vector3d(rotationRate); momentOfInertia.transform(Iw); Vector3d angularAcc = new Vector3d(); angularAcc.cross(rotationRate, Iw); angularAcc.negate(); angularAcc.add(getTorque()); momentOfInertiaInv.transform(angularAcc); angularAcc.scale(dt); rotationRate.add(angularAcc); } lastTime = t; }
Example 20
Source File: EarthVector.java From geowave with Apache License 2.0 | 3 votes |
public Vector3d getNormalizedEarthTangentVector(final double azimuth) { // TODO: rewrite this to use real math instead of this silly difference final EarthVector nextEV = findPoint(1, azimuth); final Vector3d deltaVec = new Vector3d(); deltaVec.sub(nextEV.getVector(), getVector()); deltaVec.normalize(); return deltaVec; }