List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getNorm
public double getNorm()
From source file:org.jtrfp.trcl.beh.AutoLeveling.java
/** * @param levelingVector/*from w w w . ja va 2 s. c o m*/ * the levelingVector to set */ public AutoLeveling setLevelingVector(Vector3D levelingVector) { if (levelingVector.getNorm() == 0) throw new RuntimeException("Intolerable zero leveling vector."); this.levelingVector = levelingVector.normalize(); return this; }
From source file:org.jtrfp.trcl.beh.CollidesWithTerrain.java
@Override public void _tick(long tickTimeMillis) { if (tickCounter++ % 2 == 0 && !recentlyCollided) return;/*from w w w. ja va2 s . c om*/ recentlyCollided = false; final WorldObject p = getParent(); final TR tr = p.getTr(); final World world = tr.getWorld(); final InterpolatingAltitudeMap aMap; final Mission mission = tr.getGame().getCurrentMission(); try { aMap = mission.getOverworldSystem().getAltitudeMap(); } catch (NullPointerException e) { return; } if (mission.getOverworldSystem().isTunnelMode()) return;//No terrain to collide with while in tunnel mode. if (aMap == null) return; final double[] thisPos = p.getPosition(); final double groundHeightNorm = aMap.heightAt((thisPos[0] / TR.mapSquareSize), (thisPos[2] / TR.mapSquareSize)); final double groundHeight = groundHeightNorm * (world.sizeY / 2); final double ceilingHeight = (1.99 - aMap.heightAt((thisPos[0] / TR.mapSquareSize), (thisPos[2] / TR.mapSquareSize))) * (world.sizeY / 2) + CEILING_Y_NUDGE; final Vector3D groundNormal = (aMap.normalAt((thisPos[0] / TR.mapSquareSize), (thisPos[2] / TR.mapSquareSize))); Vector3D downhillDirectionXZ = new Vector3D(groundNormal.getX(), 0, groundNormal.getZ()); if (downhillDirectionXZ.getNorm() != 0) downhillDirectionXZ = downhillDirectionXZ.normalize(); else downhillDirectionXZ = Vector3D.PLUS_J; final OverworldSystem overworldSystem = tr.getGame().getCurrentMission().getOverworldSystem(); if (overworldSystem == null) return; final boolean terrainMirror = overworldSystem.isChamberMode(); final double thisY = thisPos[1]; boolean groundImpact = thisY < (groundHeight + (autoNudge ? nudgePadding : 0)); final boolean ceilingImpact = (thisY > ceilingHeight && terrainMirror && !ignoreCeiling); final Vector3D ceilingNormal = new Vector3D(groundNormal.getX(), -groundNormal.getY(), groundNormal.getZ()); Vector3D surfaceNormal = groundImpact ? groundNormal : ceilingNormal; final double dot = surfaceNormal.dotProduct(getParent().getHeading()); if (terrainMirror && groundHeightNorm > .97) { groundImpact = true; surfaceNormal = downhillDirectionXZ; } //end if(smushed between floor and ceiling) if (groundLock) { recentlyCollided = true; thisPos[1] = groundHeight; p.notifyPositionChange(); return; } //end if(groundLock) if (tunnelEntryCapable && groundImpact && dot < 0) { final OverworldSystem os = mission.getOverworldSystem(); if (!os.isTunnelMode()) { TunnelEntranceObject teo = mission.getTunnelEntranceObject( new Point((int) (thisPos[0] / TR.mapSquareSize), (int) (thisPos[2] / TR.mapSquareSize))); if (teo != null && !mission.isBossFight()) { mission.enterTunnel(teo.getSourceTunnel()); return; } } //end if(above ground) } //end if(tunnelEntryCapable()) if (groundImpact || ceilingImpact) {// detect collision recentlyCollided = true; double padding = autoNudge ? nudgePadding : 0; padding *= groundImpact ? 1 : -1; thisPos[1] = (groundImpact ? groundHeight : ceilingHeight) + padding; p.notifyPositionChange(); if (dot < 0 || ignoreHeadingForImpact) {//If toward ground, call impact listeners. surfaceNormalVar = surfaceNormal; final Behavior behavior = p.getBehavior(); behavior.probeForBehaviors(sub, SurfaceImpactListener.class); } //end if(pointedTowardGround) } // end if(collision) }
From source file:org.jtrfp.trcl.beh.phy.BouncesOffSurfaces.java
@Override public void collidedWithSurface(WorldObject wo, double[] surfaceNormal) { final WorldObject parent = getParent(); final Vector3D oldHeading = parent.getHeading(); final Vector3D oldTop = parent.getTop(); final Vector3D _surfaceNormal = new Vector3D(surfaceNormal); if (oldHeading == null) throw new NullPointerException("Parent heading is null."); if (surfaceNormal == null) throw new NullPointerException("Surface normal is null."); if (reflectHeading && new Rotation(oldHeading, _surfaceNormal).getAngle() > Math.PI / 2.) { Vector3D newHeading = (_surfaceNormal.scalarMultiply(_surfaceNormal.dotProduct(oldHeading) * -2) .add(oldHeading));/*from www.java2 s.co m*/ parent.setHeading(newHeading); final Rotation resultingRotation = new Rotation(oldHeading, newHeading); Vector3D newTop = resultingRotation.applyTo(oldTop); //if(newTop.getY()<0)newTop=newTop.negate(); parent.setTop(newTop); } //end if(should reflect) //if(parent instanceof Velocible){ final Velocible velocible = (Velocible) parent.probeForBehavior(Velocible.class); Vector3D oldVelocity = velocible.getVelocity(); if (oldVelocity.getNorm() == 0) oldVelocity = Vector3D.PLUS_I; if (new Rotation(oldVelocity.normalize(), _surfaceNormal).getAngle() > Math.PI / 2.) { velocible.setVelocity( (_surfaceNormal.scalarMultiply(_surfaceNormal.dotProduct(oldVelocity) * -2).add(oldVelocity)) .scalarMultiply(velocityRetainmentCoefficient)); //Nudge parent.setPosition( new Vector3D(parent.getPosition()).add(_surfaceNormal.scalarMultiply(1000.)).toArray()); } //end if(should bounce) //}//end if(Velocible) }
From source file:org.jtrfp.trcl.beh.ProjectileBehavior.java
public void reset(Vector3D heading, double speed) { this.speed = speed; honingTarget = null;/*w w w. j a v a 2 s. co m*/ final WorldObject parent = getParent(); final Behavior beh = parent.getBehavior(); parent.setHeading(heading); if (honing) { // Find target WorldObject closestObject = null; double closestDistance = Double.POSITIVE_INFINITY; List<WorldObject> possibleTargets = getParent().getTr().getCollisionManager() .getCurrentlyActiveCollisionList(); synchronized (possibleTargets) { for (WorldObject possibleTarget : possibleTargets) { if (possibleTarget instanceof DEFObject) { DEFObject possibleDEFTarget = (DEFObject) possibleTarget; if (!possibleDEFTarget.isIgnoringProjectiles() && !possibleDEFTarget.isRuin()) { final Vector3D targetPos = new Vector3D(possibleTarget.getPositionWithOffset()); final Vector3D delta = targetPos.subtract(new Vector3D(getParent().getPosition())); final double dist = delta.getNorm(); final Vector3D proposedHeading = delta.normalize(); final Vector3D headingDelta = getParent().getHeading().subtract(proposedHeading); final double compositeHeadingDelta = headingDelta.getNorm(); if (compositeHeadingDelta < .5) { final double compositeDistance = dist; if (compositeDistance < closestDistance) { closestDistance = dist; closestObject = possibleTarget; parent.setHeading(proposedHeading); getParent().getBehavior().probeForBehavior(AutoLeveling.class) .setLevelingVector(heading); } // end if(closesObject) } // end if(headingDelta<1) } // end if(isIgnoringProjectiles) } // end if(DEFObject) } } // end for(WorldObject others) honingTarget = new WeakReference<WorldObject>(closestObject); // if(honingTarget==null){ getParent().getBehavior().probeForBehavior(AutoLeveling.class).setLevelingVector(heading); movesByVelocity.setVelocity(getParent().getHeading().scalarMultiply(speed)); // }//end if(honingTarget==null) } // end if(honingTarget) beh.probeForBehavior(LimitedLifeSpan.class).reset(LIFESPAN_MILLIS); beh.probeForBehavior(DeathBehavior.class).reset(); }
From source file:org.jtrfp.trcl.core.ResourceManager.java
public Model getBINModel(String name, TextureDescription defaultTexture, double scale, boolean cache, ColorPaletteVectorList palette, ColorPaletteVectorList ESTuTvPalette) throws FileLoadException, IOException, IllegalAccessException { if (name == null) throw new NullPointerException("Name is intolerably null"); if (palette == null) throw new NullPointerException("Palette is intolerably null"); if (modelCache.containsKey(name) && cache) return modelCache.get(name); //The models like to set up two line segments where there should be one. //This set is for identifying and culling redundant segs. final HashSet<Integer> alreadyVisitedLineSegs = new HashSet<Integer>(); boolean hasAlpha = false; try {/* ww w . ja v a2s . c o m*/ BINFile.AnimationControl ac = null; Model result = new Model(true, tr); ac = aniBinNameMap.get(name); if (ac == null) { InputStream is = getInputStreamFromResource("MODELS\\" + name); //TODO: InputStream not guaranteed to close when exception is thrown. Wrap in try{}, close it, and re-throw. ac = new BINFile.AnimationControl(is);//This will throw an exception on and escape to the static model block is.close(); aniBinNameMap.put(name, ac); } System.out.println("Recognized as animation control file."); //Build the Model from the BINFile.Model Model[] frames = new Model[ac.getNumFrames()]; for (int i = 0; i < frames.length; i++) { frames[i] = getBINModel(ac.getBinFiles().get(i), defaultTexture, scale, cache, palette, ESTuTvPalette); } result.setDebugName(name + " triangles: " + frames[0].getRawTriangleLists().get(0).size()); //Consolidate the frames to one model for (int i = 0; i < frames.length; i++) { result.addFrame(frames[i]); } result.setFrameDelayInMillis((int) (((double) ac.getDelay() / 65535.) * 1000.)); //result.finalizeModel(); if (cache) modelCache.put(name, result); return result; } //end try{} catch (UnrecognizedFormatException e) {//ok fail. Static model try { BINFile.Model m = null; Model result = new Model(false, tr); result.setDebugName(name); m = modBinNameMap.get(name); if (m == null) { InputStream is = getInputStreamFromResource("MODELS\\" + name); m = new BINFile.Model(is); modBinNameMap.put(name, m); } //end if(null) final double cpScalar = (scale * TR.crossPlatformScalar * 256.) / (double) m.getScale(); System.out.println("Recognized as model file."); List<org.jtrfp.trcl.gpu.Vertex> vertices = new ArrayList<org.jtrfp.trcl.gpu.Vertex>(); for (BINFile.Model.Vertex binVtx : m.getVertices()) { vertices.add(new org.jtrfp.trcl.gpu.Vertex().setPosition(new Vector3D(binVtx.getX() * cpScalar, binVtx.getY() * cpScalar, binVtx.getZ() * cpScalar))); } //end try{} TextureDescription currentTexture = null; final double[] u = new double[4]; final double[] v = new double[4]; for (ThirdPartyParseable b : m.getDataBlocks()) { //Sort out types of block if (b instanceof TextureBlock) { TextureBlock tb = (TextureBlock) b; if (hasAlpha) currentTexture = getRAWAsTexture(tb.getTextureFileName(), palette, ESTuTvPalette, hasAlpha); else { currentTexture = getRAWAsTexture(tb.getTextureFileName(), palette, ESTuTvPalette, false); } System.out.println( "ResourceManager: TextureBlock specifies texture: " + tb.getTextureFileName()); } //end if(TextureBlock) else if (b instanceof FaceBlock) { //System.out.println("FaceBlock found: "+b.getClass().getSimpleName()); FaceBlock block = (FaceBlock) b; List<FaceBlockVertex> vertIndices = block.getVertices(); if (currentTexture == null) { System.out.println("Warning: Face texture not specified. Using fallback texture."); currentTexture = defaultTexture; } /* * "The two vb_tex_coord values map the vertices of the face to the texture. * They are both in the range of 0x0 to 0xFF00, with u=0x0, v=0x0 being the upper * left corner of the texture, and u=0xFF00, v=0xFF00 being the lower right corner." * - http://www.viaregio.de/pieper/mtm/bin_file_format.shtml */ //// Note: It appears that Stefan's 0xFF0000 approach works rather than the 0xFF00 value. typo? if (vertIndices.size() == 4) {//Quads org.jtrfp.trcl.gpu.Vertex[] vtx = new org.jtrfp.trcl.gpu.Vertex[4]; for (int i = 0; i < 4; i++) { vtx[i] = vertices.get(vertIndices.get(i).getVertexIndex() % (b instanceof FaceBlock05 ? 10 : Integer.MAX_VALUE)); } Vector3D blockNormal = new Vector3D(block.getNormalX(), block.getNormalY(), block.getNormalZ()); if (blockNormal.getNorm() == 0) blockNormal = new Vector3D(1, 0, 0);//Use filler if zero norm. if (vertIndices.get(0) instanceof FaceBlockVertexWithUV) { for (int i = 0; i < 4; i++) { final FaceBlockVertexWithUV fbvi = (FaceBlockVertexWithUV) vertIndices.get(i); u[i] = (double) (fbvi).getTextureCoordinateU() / (double) 0xFF0000; v[i] = (double) (fbvi).getTextureCoordinateV() / (double) 0xFF0000; } //end for(4) } else { u[0] = BOX_U[0]; v[0] = BOX_V[0]; u[1] = BOX_U[1]; v[1] = BOX_V[1]; u[2] = BOX_U[2]; v[2] = BOX_V[2]; u[3] = BOX_U[3]; v[3] = BOX_V[3]; } Triangle[] tris = Triangle.quad2Triangles(vtx, new Vector2D[] { new Vector2D(u[0], 1. - v[0]), new Vector2D(u[1], 1. - v[1]), new Vector2D(u[2], 1. - v[2]), new Vector2D(u[3], 1. - v[3]) }, currentTexture, RenderMode.DYNAMIC, hasAlpha, blockNormal.normalize(), "quad.BINmodel" + name); result.addTriangle(tris[0]); result.addTriangle(tris[1]); } else if (vertIndices.size() == 3)//Triangles { Triangle t = new Triangle(currentTexture); try { t.setCentroidNormal( new Vector3D(block.getNormalX(), block.getNormalY(), block.getNormalZ()) .normalize()); } catch (MathArithmeticException ee) { t.setCentroidNormal(Vector3D.ZERO); } t.setAlphaBlended(hasAlpha); t.setRenderMode(RenderMode.DYNAMIC); for (int vi = 0; vi < 3; vi++) { final org.jtrfp.trcl.gpu.Vertex vtx = vertices .get(vertIndices.get(vi).getVertexIndex() - (b instanceof FaceBlock05 ? m.getUnknown2() : 0)); t.setVertex(vtx, vi); if (b instanceof FaceBlock05 || !(vertIndices.get(0) instanceof FaceBlockVertexWithUV)) t.setUV(new Vector2D(BOX_U[vi], BOX_V[vi]), vi); else { t.setUV(new Vector2D( (double) ((FaceBlockVertexWithUV) vertIndices.get(vi)) .getTextureCoordinateU() / (double) 0xFF0000, 1. - (double) ((FaceBlockVertexWithUV) vertIndices.get(vi)) .getTextureCoordinateV() / (double) 0xFF0000), vi); } } //end for(vi) if (currentTexture == null) { System.err.println("WARNING: Texture never set for " + name + ". Using fallback."); currentTexture = tr.gpu.get().textureManager.get().getFallbackTexture(); } result.addTriangle(t); } //end if(3 vertices) else { System.err.println("ResourceManager: FaceBlock has " + vertIndices.size() + " vertices. Only 3 or 4 supported."); } } //end if(FaceBlock) else if (b instanceof ColorBlock) { final ColorBlock cb = (ColorBlock) b; final byte[] bytes = cb.getBytes(); final Color color = new Color(bytes[0] & 0xFF, bytes[1] & 0xFF, bytes[2] & 0xFF); currentTexture = tr.gpu.get().textureManager.get().solidColor(color); } else if (b instanceof FaceBlock19) { System.out.println(b.getClass().getSimpleName() + " (solid colored faces) not yet implemented. Skipping..."); } else if (b instanceof FaceBlock05) { } //TODO else if (b instanceof LineSegmentBlock) { LineSegmentBlock block = (LineSegmentBlock) b; org.jtrfp.trcl.gpu.Vertex v1 = vertices.get(block.getVertexID1()); org.jtrfp.trcl.gpu.Vertex v2 = vertices.get(block.getVertexID2()); if (!alreadyVisitedLineSegs.contains(v1.hashCode() * v2.hashCode())) { Triangle[] newTris = new Triangle[6]; LineSegment.buildTriPipe(v1.getPosition(), v2.getPosition(), tr.gpu.get().textureManager.get().getDefaultTriPipeTexture(), 200, newTris, 0); result.addTriangles(newTris); alreadyVisitedLineSegs.add(v1.hashCode() * v2.hashCode()); } //end if(not already visited) } //end if(LineSegmentBlock) else if (b instanceof Unknown12) { System.out.println("Found unknown12. Assuming this is a tag for a transparent texture..."); hasAlpha = true; } else if (b instanceof AnimatedTextureBlock) { System.out.println("Found animated texture block."); AnimatedTextureBlock block = (AnimatedTextureBlock) b; List<String> frames = block.getFrameNames(); double timeBetweenFramesInMillis = ((double) block.getDelay() / 65535.) * 1000.; Texture[] subTextures = new Texture[frames.size()]; for (int ti = 0; ti < frames.size(); ti++) { if (!hasAlpha) subTextures[ti] = (Texture) getRAWAsTexture(frames.get(ti), palette, ESTuTvPalette, false); else subTextures[ti] = (Texture) getRAWAsTexture(frames.get(ti), palette, ESTuTvPalette, true); //subTextures[ti]=tex instanceof Texture?new DummyTRFutureTask<Texture>((Texture)tex):(Texture)Texture.getFallbackTexture(); } //end for(frames) //fDelay, nFrames,interp currentTexture = new AnimatedTexture( new Sequencer((int) timeBetweenFramesInMillis, subTextures.length, false), subTextures); } else if (b instanceof EOFBlock) { System.out.println("...That's all, end of BIN"); } else { System.out.println("Failed to identify DataBlock: " + b.getClass().getName()); } } //end for(dataBlocks) //result.finalizeModel(); result.setDebugName(name); //if(result.getTriangleList()==null && result.getTransparentTriangleList()==null) // throw new RuntimeException("Resulting BIN has no triangleList"); if (cache) modelCache.put(name, result); return result; } //end try{} catch (UnrecognizedFormatException ee) { //Not-good fail throw new UnrecognizedFormatException( "Can't figure out what this is: " + name + ". Giving up. Expect trouble ahead."); } } //end catch(ok fail) //Bad fail. }
From source file:org.jtrfp.trcl.obj.ProjectileBillboard.java
public void reset(double[] newPos, Vector3D newVelocity, WorldObject objectOfOrigin) { this.objectOfOrigin = new WeakReference<WorldObject>(objectOfOrigin); getBehavior().probeForBehavior(LimitedLifeSpan.class).reset(LIFESPAN_MILLIS); setHeading(newVelocity.normalize()); setPosition(newPos[0], newPos[1], newPos[2]); setVisible(true);/* w w w . j a va 2 s .c om*/ setActive(true); getBehavior().probeForBehavior(Velocible.class).setVelocity(newVelocity); getBehavior().probeForBehavior(ProjectileBehavior.class).reset(newVelocity.normalize(), newVelocity.getNorm()); }
From source file:org.jtrfp.trcl.obj.ProjectileFactory.java
public Projectile fire(double[] newPosition, Vector3D heading, WorldObject objectOfOrigin) { assert !Vect3D.isAnyNaN(newPosition); assert heading.getNorm() != 0 && heading.getNorm() != Double.NaN; final Projectile result = projectiles[projectileIndex]; result.destroy();//from w w w.ja va2 s. c o m result.reset(newPosition, heading.scalarMultiply(projectileSpeed), objectOfOrigin); ((WorldObject) result).setTop(objectOfOrigin.getTop()); tr.getDefaultGrid().add((WorldObject) result); tr.mainRenderer.get().temporarilyMakeImmediatelyRelevant((PositionedRenderable) result); if (soundTexture != null) tr.soundSystem.get() .enqueuePlaybackEvent(tr.soundSystem.get().getPlaybackFactory().create(soundTexture, (WorldObject) result, tr.mainRenderer.get().getCamera(), (objectOfOrigin instanceof Player ? .6 : 1) * SoundSystem.DEFAULT_SFX_VOLUME));//TODO: Use configuration volume instead final List<WorldObject> cL = tr.getCollisionManager().getCurrentlyActiveCollisionList(); synchronized (cL) { cL.add((WorldObject) result); } projectileIndex++; projectileIndex %= projectiles.length; return result; }
From source file:org.jtrfp.trcl.obj.ProjectileObject3D.java
@Override public void reset(double[] newPos, Vector3D newVelocity, WorldObject objectOfOrigin) { this.objectOfOrigin = new WeakReference<WorldObject>(objectOfOrigin); if (newVelocity.getNorm() != 0) setHeading(newVelocity.normalize()); else {/* www.ja va 2s .co m*/ setHeading(Vector3D.PLUS_I); newVelocity = Vector3D.PLUS_I; } //meh. assert !Vect3D.isAnyNaN(newPos); setPosition(newPos[0], newPos[1], newPos[2]); getBehavior().probeForBehavior(Velocible.class).setVelocity(newVelocity); getBehavior().probeForBehavior(ProjectileBehavior.class).reset(newVelocity.normalize(), newVelocity.getNorm()); setActive(true); setVisible(true); }
From source file:org.jtrfp.trcl.obj.TunnelExitObject.java
public TunnelExitObject(TR tr, Tunnel tun) { super(tr);/*from w ww .j av a 2 s .c o m*/ addBehavior(new TunnelExitBehavior()); final DirectionVector v = tun.getSourceTunnel().getExit(); final double EXIT_Y_NUDGE = 0; final InterpolatingAltitudeMap map = tr.getGame().getCurrentMission().getOverworldSystem().getAltitudeMap(); final double exitY = map.heightAt(TR.legacy2Modern(v.getZ()), TR.legacy2Modern(v.getX())) + EXIT_Y_NUDGE; this.exitLocation = new Vector3D(TR.legacy2Modern(v.getZ()), exitY, TR.legacy2Modern(v.getX())); this.tun = tun; exitHeading = map.normalAt(exitLocation.getZ() / TR.mapSquareSize, exitLocation.getX() / TR.mapSquareSize); Vector3D horiz = exitHeading.crossProduct(Vector3D.MINUS_J); if (horiz.getNorm() == 0) { horiz = Vector3D.PLUS_I; } else horiz = horiz.normalize(); exitTop = exitHeading.crossProduct(horiz.negate()).normalize().negate(); exitLocation = exitLocation.add(exitHeading.scalarMultiply(10000)); this.tr = tr; setVisible(false); try { Model m = tr.getResourceManager().getBINModel("SHIP.BIN", tr.getGlobalPaletteVL(), null, tr.gpu.get().getGl()); setModel(m); } catch (Exception e) { e.printStackTrace(); } }
From source file:org.orekit.attitudes.BodyCenterPointingTest.java
@Test public void testSpin() throws OrekitException { Utils.setDataRoot("regular-data"); final double ehMu = 3.9860047e14; final double ae = 6.378137e6; final double c20 = -1.08263e-3; final double c30 = 2.54e-6; final double c40 = 1.62e-6; final double c50 = 2.3e-7; final double c60 = -5.5e-7; final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.); final Vector3D position = new Vector3D(3220103., 69623., 6449822.); final Vector3D velocity = new Vector3D(6414.7, -2006., -3180.); final CircularOrbit initialOrbit = new CircularOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, ehMu); EcksteinHechlerPropagator propagator = new EcksteinHechlerPropagator(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);//from ww w .jav a 2 s .c o m propagator.setAttitudeProvider(earthCenterAttitudeLaw); double h = 0.01; SpacecraftState s0 = propagator.propagate(date); SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h)); SpacecraftState sPlus = propagator.propagate(date.shiftedBy(h)); // check spin is consistent with attitude evolution double errorAngleMinus = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(), s0.getAttitude().getRotation()); double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(), s0.getAttitude().getRotation()); Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus); double errorAnglePlus = Rotation.distance(s0.getAttitude().getRotation(), sPlus.shiftedBy(-h).getAttitude().getRotation()); double evolutionAnglePlus = Rotation.distance(s0.getAttitude().getRotation(), sPlus.getAttitude().getRotation()); Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus); Vector3D spin0 = s0.getAttitude().getSpin(); Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(), sPlus.getAttitude().getRotation(), 2 * h); Assert.assertTrue(spin0.getNorm() > 1.0e-3); Assert.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-13); }