/** * * This is a robust collision of a's adjacent edges with a horizontal plane at the given height * When Two parallel edges are given, we can assume that the */ public static Tuple3d collide (Corner a, double height) { LinearForm3D ceiling = new LinearForm3D( 0, 0, 1, -height ); // this can cause Jama not to return... if ( a.prevL.linearForm.hasNaN() || a.nextL.linearForm.hasNaN() ) throw new Error(); try { return ceiling.collide(a.prevL.linearForm, a.nextL.linearForm); } catch (RuntimeException e) { assert (a.prevL.sameDirectedLine(a.nextL)); // a vector in the direction of uphill from a Vector3d dir = new Vector3d ( a.prevL.uphill ); dir.normalize(); // via similar triangle (pyramids) dir.scale(height - a.z); dir.add(a); //assume, they're all coincident? return new Point3d (dir.x, dir.y, height); } }
/** * @see http://pl.wikipedia.org/wiki/Elementarne_macierze_transformacji * @param pTuple3d * @param pAngle * @return */ public static Vector3d rotateX3d(Tuple3d pTuple, double pAngle) { double x = pTuple.x; double y = pTuple.y; double z = pTuple.z; double cosA = Math.cos(pAngle); double sinA = Math.sin(pAngle); return new Vector3d( x, y * cosA - z * sinA, y * sinA + z * cosA); }
/** * @see http://pl.wikipedia.org/wiki/Elementarne_macierze_transformacji * @param pTuple3d * @param pAngle * @return */ public static Vector3d rotateY3d(Tuple3d pTuple3d, double pAngle) { double x = pTuple3d.x; double y = pTuple3d.y; double z = pTuple3d.z; double cosB = Math.cos(pAngle); double sinB = Math.sin(pAngle); return new Vector3d( x * cosB + z * sinB, y, -x * sinB + z * cosB); }
public Matrix4f computeSurfaceTransform( double lat, double lon, double hEllps, double az, double ha, Matrix4f mat, Tuple3d origin) { if (mat == null) { mat = new Matrix4f(); } else { mat.m01 = mat.m10 = mat.m02 = mat.m20 = mat.m12 = mat.m21 = mat.m30 = mat.m31 = mat.m32 = 0; } mat.m00 = mat.m11 = mat.m22 = mat.m33 = 1; synchronized (eye) { toCartesian(lat, lon, hEllps, eye); mat.m03 = (float)(eye.x - origin.x); mat.m13 = (float)(eye.y - origin.y); mat.m23 = (float)(eye.z - origin.z); } synchronized (workTrans) { workTrans.rotZ((float)(Math.PI/2.+lon)); mat.mul(workTrans); workTrans.rotX((float)(Math.PI/2.-lat)); mat.mul(workTrans); workTrans.rotZ((float)(-az)); mat.mul(workTrans); workTrans.rotX((float)(Math.PI/2.+ha)); mat.mul(workTrans); } return mat; }
/** When called, the bot starts steering, when possible, he get's nearer the target location. */ @Override public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) { // Supposed velocity in the next tick of logic, after applying various steering forces to the bot. Vector3d nextVelocity = new Vector3d(0,0,0); for(Target_packet tp : targets) { /** ISteering properties: target location - bot approaches this location. */ Location targetLocation = tp.getTargetLocation(); // A vector from the bot to the target location. Vector3d vectorToTarget = new Vector3d(targetLocation.x - botself.getLocation().x, targetLocation.y - botself.getLocation().y, 0); double distFromTarget = vectorToTarget.length(); /** ISteering properties: target gravity - a parameter meaning how attracted the bot is to his target location. */ int attractiveForce = tp.getAttractiveForce(distFromTarget); if (distFromTarget < NEARLY_THERE_DISTANCE) { wantsToStop.setValue(true); //if (SteeringManager.DEBUG) System.out.println("We reached the target"); } else { vectorToTarget.normalize(); vectorToTarget.scale(attractiveForce); nextVelocity.add((Tuple3d) vectorToTarget); } } wantsToGoFaster.setValue(true); return nextVelocity; }
public FacialPoint(FacialPointType type, Tuple3d coords) { this.pos = new Vector3f(coords); this.type = type; this.name = FpTexter.getInstance().getFPname(type); this.info = FpTexter.getInstance().getFPinfo(type); }
public int compare( Chain o1, Chain o2 ) { Corner c1 = o1.chain.get( 0 ); Corner c2 = o2.chain.get( 0 ); // except for the first and and last point // chain's non-start/end points are always at the position of the collision - so to // find the angle of the first edge at the specified height, we project it's start // coordinate the desired height and take the angle relative to the collision // !could speed up with a chain-class that caches this info! // try // { Tuple3d p1 = Edge.collide(c1, height);//ceiling.collide( c1.prevL.linearForm, c1.nextL.linearForm ); Tuple3d p2 = Edge.collide(c2, height );//ceiling.collide( c2.prevL.linearForm, c2.nextL.linearForm ); p1.sub( loc ); p2.sub( loc ); // start/end line is (+-)Pi return Double.compare( Math.atan2( p1.y, p1.x ), Math.atan2( p2.y, p2.x ) ); // } // catch (RuntimeException e) // { // // we can probably fix these up (by assuming that they're horizontal?) // // todo: can we prove they are safe to ignore? eg: no parallel edges inbound, none outbound etc.. // System.err.println( "didn't like colliding 1" + c1.prevL + " and " + c1.nextL ); // System.err.println( " 2" + c2.prevL + " and " + c2.nextL ); // return 0; // } }
public void addOutputSideTo( boolean isTop, Tuple3d a, Tuple3d b, Edge... edges ) // a.y == b.yequals(b) { for ( Edge edge : edges ) { Corner c = edge.start; // assumption: start of edge will always be a leading corner on the face! Face f = faces.get( c ); assert (f != null); if (isTop) f.topSE.add( createEdge( a, b ) ); // just check those tuple's aren't corners.... f.results.add( new Point3d( a ), new Point3d( b ) ); // System.out.println(">>"); } }
private SharedEdge createEdge ( Tuple3d start, Tuple3d end) { SharedEdge newEdge = new SharedEdge (new Point3d (start), new Point3d (end )); newEdge = edges.get( newEdge ); // identity lookup - only one edge! return newEdge; }
public void addCube (Tuple3d corner, Tuple3d up, Tuple3d along, double upL, double alongL, double inL ) { Vector3d in = new Vector3d(); in.cross( new Vector3d(up), new Vector3d(along) ); addCube ( Jme3z.to( corner ), Jme3z.to( up ).normalizeLocal(), Jme3z.to( along ).normalizeLocal(), Jme3z.to( in ).normalizeLocal(), (float) upL, (float) alongL, (float) inL ); }
private String printTuple3d( Tuple3d t ) { StringBuilder buf = new StringBuilder( "<" ); buf .append( FORMAT.format(t.x) ); buf .append( "," ); buf .append( FORMAT.format(t.y) ); buf .append( "," ); buf .append( FORMAT.format(t.z) ); buf .append( ">" ); return buf .toString(); }
public static void checkReal(Tuple3d p) { if (Double.isNaN(p.x) || Double.isNaN(p.y) || Double.isNaN(p.z)) { throw new IllegalArgumentException("tuple has NaN values: " + p); } if (Double.isInfinite(p.x) || Double.isInfinite(p.y) || Double.isInfinite(p.z)) { throw new IllegalArgumentException("tuple has Infinite values: " + p); } }
/** * Sets the camera location to <code>newLocation</code> without affecting the rotation. */ public void setLocation( Tuple3d newLocation , boolean applyNow ) { m_pendingPosition.setLocation( newLocation ); if( applyNow ) { exportVPT( ); } }
static void checkValid( Tuple3d t ) { if( Double.isNaN( t.x ) || Double.isNaN( t.y ) || Double.isNaN( t.z ) ) { throw new IllegalArgumentException( "tuple has NaN values" ); } if( Double.isInfinite( t.x ) || Double.isInfinite( t.y ) || Double.isInfinite( t.z ) ) { throw new IllegalArgumentException( "tuple has Infinite values" ); } }
/** * Sets the camera location to <code>newLocation</code> without affecting the rotation. */ public void setLocation( Tuple3d newLocation ) { checkValid( newLocation ); location.set( newLocation ); xform.setTranslation( location ); invXformUpToDate = false; locationUpToDate = true; }
protected static String getJmolPoint(Tuple3d point) { StringBuilder s = new StringBuilder(); s.append("{"); s.append(fDot2(point.x)); s.append(","); s.append(fDot2(point.y)); s.append(","); s.append(fDot2(point.z)); s.append("}"); return s.toString(); }
public final void toCartesian(double lat, double lon, double h, Tuple3d p) { double n = getN(lat); double r = (n+h) * Math.cos(lat); p.x = r * Math.cos(lon); p.y = r * Math.sin(lon); p.z = (n * (1.-getE2()) + h) * Math.sin(lat); }
/** * The main method. This method must be called in each tick (logic), if we want the navigation layer to compute the next velocity and send it to the locomotion layer. * Note: Should not be called anymore. Use start() and stop() methods. */ public void run() { steeringForces.clear(); Vector3d velocity = botself.getVelocity().getVector3d(); if (SteeringManager.DEBUG) System.out.println("Velocity "+velocity+" length "+velocity.length()); // Supposed velocity in the next tick of logic, after applying various steering forces to the bot. Vector3d nextVelocity = new Vector3d(velocity.x, velocity.y, velocity.z); double actualWeight; if (useLastVeloWeight) { actualWeight = lastVeloWeight; } else { actualWeight = 3 - velocity.length()/WALK_VELOCITY_LENGTH; //This causes that <= WALK_VEOCITY_LENGTH will have actualWeight 2, sth. >= 2*WALK_VELOCITY_LENGTH 1, and other values wil be between 1 and 2. if (actualWeight <1) actualWeight = 1; else if (actualWeight > 2) actualWeight = 2; if (velocity.length() == 0) actualWeight = 0; } //The actual velocity has bigger weigh ==> the behavior will be smoother. //5389.0,-6203.0,-3446.65 nextVelocity.scale(actualWeight); myActualVelocity = new Vector3d(nextVelocity.x, nextVelocity.y, nextVelocity.z); Vector3d myStopVelocity = new Vector3d(nextVelocity.x, nextVelocity.y, nextVelocity.z); double totalWeight = actualWeight; boolean everyoneWantsToGoFaster = canEnlargeVelocity; RefBoolean wantsToGoFaster = new RefBoolean(false); RefBoolean wantsToStop = new RefBoolean(false); Location focusLoc = new Location(0,0,0); for(SteeringType stType : mySteerings.keySet()) { ISteering steering = mySteerings.get(stType); RefLocation newFocus = new RefLocation(); newFocus.data = new Location(0, 0, 0); Vector3d newVelocity = setVelocitySpecific(steering, wantsToGoFaster, wantsToStop, newFocus); focusLoc = setFocusSpecific(stType,wantsToStop.getValue(),newFocus.data,focusLoc); if (wantsToStop.getValue()) { //Wants to stop causes, tak bot stops, if this steering is the only one. Otherwise the other steerings can cause that bot will again move. newVelocity.x = -myStopVelocity.x; newVelocity.y = -myStopVelocity.y; newVelocity.z = -myStopVelocity.z; myStopVelocity.sub(newVelocity); everyoneWantsToGoFaster = false; if (SteeringManager.DEBUG) System.out.println("We stop."); wantsToStop.setValue(false); } else { if (newVelocity.length() > MAX_FORCE) newVelocity.scale(MAX_FORCE/newVelocity.length()); newVelocity.scale(steeringWeights.get(stType)); //Each steering has its own weight. everyoneWantsToGoFaster = everyoneWantsToGoFaster && wantsToGoFaster.getValue(); } if (newVelocity.length()>0) { //TODO: WARNING hack to use different type of steering return values //it should be redone, more cleaner and robust way... Petr B. newVelocity.add((Tuple3d)nextVelocity); nextVelocity = newVelocity; if (newVelocity.length() > MIN_VALUE_TO_SUM) //Only significant steerings are counted into totalWeight. totalWeight += steeringWeights.get(stType); } if (SteeringManager.DEBUG) System.out.println(steering.toString()+"| length "+newVelocity.length()+" | weight: "+steeringWeights.get(stType)); steeringForces.put(stType, newVelocity); } if (SteeringManager.DEBUG) System.out.print("Sum "+nextVelocity.length()+" TotalWeight: "+totalWeight); if (totalWeight > 0) { nextVelocity.scale(1/totalWeight); } if (SteeringManager.DEBUG) System.out.println(" Result "+nextVelocity.length()); moveTheBot(nextVelocity, everyoneWantsToGoFaster, focusLoc); }
@Override public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) { // <editor-fold defaultstate="collapsed" desc="debug"> if (properties == null) { if (SOC_STEER_LOG.DEBUG) { SOC_STEER_LOG.AddLogLineWithDate("no properties", "triangleError"); } }// </editor-fold> Location newFocus = getFocus(); if (newFocus != null) { focus.data = newFocus; } //returns ideal place where steered agent wants to stay... Location targetLocation = WhereToGo(botself, properties); // Supposed velocity in the next tick of logic, after applying various steering forces to the bot. SteeringResult nextVelocity = new SteeringResult(new Vector3d(0, 0, 0), 1); //we are able to compute ideal place... if (targetLocation != null) { // A vector from the bot to the target location. targetLocation = new Location(targetLocation.x,targetLocation.y, botself.getLocation().z); Vector3d vectorToTarget = targetLocation.sub(botself.getLocation()).asVector3d(); double distFromTarget = vectorToTarget.length(); nextVelocity.setMult(distFromTarget / 100); if (distFromTarget < KMinimalDistance) { wantsToStop.setValue(true); return new SteeringResult(new Vector3d(0, 0, 0), 1); } double attractiveForce = KDefaultAttraction;//* (distFromTarget / KDefaultAttractionDistance); vectorToTarget.normalize(); vectorToTarget.scale(attractiveForce); nextVelocity.add((Tuple3d) vectorToTarget); }else { nextVelocity.setMult(1); } //no need to scale, scaling is done within method attraction(...) int botAttractiveForce = KDefaultAttraction / 6; Vector3d attractionFromFst = attraction(botself, properties.getFstBot(), 1.3); Vector3d attractionFromSnd = attraction(botself, properties.getSndBot(), 1.3); attractionFromFst.scale(botAttractiveForce); nextVelocity.add((Tuple3d) attractionFromFst); attractionFromSnd.scale(botAttractiveForce); nextVelocity.add((Tuple3d) attractionFromSnd); wantsToGoFaster.setValue(false); return nextVelocity; }
public Box3D(Tuple3d p1, Tuple3d p2) { this(p1.getX(), p2.getX(), p1.getY(), p2.getY(), p1.getZ(), p2.getZ()); }
/** * Instances of Point3D are directly added, other Point are converted to * Point3D with the same location. */ public PointSet3D(Tuple3d[] points) { for (Tuple3d element : points) this.points.add(new Point3D(element)); }
public Point3D(Tuple3d point) { this(point.getX(), point.getY(), point.getZ()); }
public DebugGen( List<Tuple3d> todebug2, Tweed tweed ) { super( "foo", tweed ); this.cubes = todebug2; }
public static Point2d to2( Tuple3d loc ) { return new Point2d(loc.x, loc.z); }
public static void convertToMini( Iterable<File> bigObj, File outfile, Matrix4d transform ) { outfile.mkdirs(); ObjDump src = new ObjDump( bigObj );//.iterator().next() ); src.centerVerts(); src.transform (transform); long count = src.material2Face.entrySet().stream().mapToInt( x -> x.getValue().size() ).sum(); double[] bounds = src.orderVert.stream().collect( new InaxPoint3dCollector() ); long targetCount = 5000; double volume = ( bounds[ 1 ] - bounds[ 0 ] ) * ( bounds[ 3 ] - bounds[ 2 ] ) * ( bounds[ 5 ] - bounds[ 4 ] ); double edgeLength = Math.pow( volume / ( count / targetCount ), 0.3333 ); int xc = (int) Math.ceil( ( bounds[ 1 ] - bounds[ 0 ] ) / edgeLength ), yc = (int) Math.ceil( ( bounds[ 3 ] - bounds[ 2 ] ) / edgeLength ), zc = (int) Math.ceil( ( bounds[ 5 ] - bounds[ 4 ] ) / edgeLength ); Set<Face>[][][] faces = new Set[xc][yc][zc]; for ( Entry<Material, List<Face>> e : src.material2Face.entrySet() ) for ( Face f : e.getValue() ) { Tuple3d vt = src.orderVert.get( f.vtIndexes.get( 0 ) ); int ix = (int) ( ( vt.x - bounds[ 0 ] ) / edgeLength ); int iy = (int) ( ( vt.y - bounds[ 2 ] ) / edgeLength ); int iz = (int) ( ( vt.z - bounds[ 4 ] ) / edgeLength ); if ( faces[ ix ][ iy ][ iz ] == null ) faces[ ix ][ iy ][ iz ] = new HashSet(); faces[ ix ][ iy ][ iz ].add( f ); } int dir = 0; MiniTransform mt = new MiniTransform(); for ( int x = 0; x < xc; x++ ) for ( int y = 0; y < yc; y++ ) for ( int z = 0; z < zc; z++ ) { Set<Face> miniF = faces[ x ][ y ][ z ]; if ( miniF == null ) continue; Matrix4d trans = new Matrix4d(); trans.setIdentity(); trans.setScale( edgeLength / 255 ); trans.setTranslation( new Vector3d( x * edgeLength + bounds[0], y * edgeLength + bounds[2], z * edgeLength + bounds[4] ) ); Matrix4d pack = new Matrix4d( trans ); pack.invert(); ObjDump mini = new ObjDump(); miniF.stream().forEach( f -> mini.addFaceFrom( f, src ) ); mini.transform( pack ); mini.dump( new File( new File( outfile, "" + dir ), OBJ ) ); mt.index.put( dir, trans ); dir++; } try { new XStream().toXML( mt, new FileWriter( new File( outfile, INDEX ) ) ); } catch ( IOException e1 ) { e1.printStackTrace(); } System.out.println( "wrote " + count + " faces to " + dir + " meshes" ); }
public Triangle(Tuple3d a, Tuple3d b, Tuple3d c) { this.a = new Point3f(a); this.b = new Point3f(b); this.c = new Point3f(c); }
public void setA(Tuple3d a) { this.a = new Point3f(a); }
public void setB(Tuple3d b) { this.b = new Point3f(b); }
public void setC(Tuple3d c) { this.c = new Point3f(c); }
public static Vector3d vectorSubtract(Tuple3d t0, Tuple3d t1){ return new Vector3d(t0.x - t1.x, t0.y- t1.y, t0.z - t1.z); }
public static Point3d pointAdd(Tuple3d t0, Tuple3d t1){ return new Point3d(t0.x + t1.x, t0.y+ t1.y, t0.z + t1.z); }
public void addOutputSideTo( Tuple3d a, Tuple3d b, Edge... edges ) { addOutputSideTo( false, a, b, edges ); }
public Corner( Tuple3d in ) { super( in ); }
private void cornerEdgeCollision( Corner corner, Edge edge ) { // check for the uphill vector of both edges being too similar (parallel edges) // also rejects e == corner.nextL or corner.prevL // updated to take into account vertical edges - will always have same uphill! - (so we check edge direction too) if ( (edge.uphill.angle( corner.prevL.uphill ) < 0.0001 && edge.direction().angle( corner.prevL.uphill) < 0.0001 ) || (edge.uphill.angle( corner.nextL.uphill ) < 0.0001 && edge.direction().angle( corner.nextL.uphill) < 0.0001 )) return; Tuple3d res = null; try { // sometimes locks up here if edge.linear form has NaN components. if ( corner.prevL.linearForm.hasNaN() || corner.nextL.linearForm.hasNaN() || edge.linearForm.hasNaN()) throw new Error(); res =edge.linearForm.collide( corner.prevL.linearForm, corner.nextL.linearForm ); } catch ( Throwable f ) { //trying to collide parallel-ish faces, don't bother // System.err.println( "didn't like colliding " + edge + " and " + corner.prevL + " and " + corner.nextL ); return; } if ( res != null ) { // cheap reject: if collision is equal or below (not the correct place to check) the corner, don't bother with it if ( res.z < corner.z || res.z < edge.start.z ) return; EdgeCollision ec = new EdgeCollision( new Point3d(res), corner.prevL, corner.nextL, edge); if (! skel.seen.contains( ec )) faceEvents.offer( ec ); } }
public static com.jme3.math.Vector3f toJmeVec( Tuple3d a ) { return new com.jme3.math.Vector3f( (float) a.x, (float) a.y, (float) a.z ); }
public static Vector3f to( Tuple3d l ) { return new Vector3f( (float) l.x, (float) l.y, (float) l.z ); }
public static Point3d convert (Tuple3d in) { Point3d out = new Point3d(in); transform.transform( out ); return out; }
public static Vector3f toF (Tuple3d in) { return new Vector3f ((float)in.x,(float)in.y,(float)in.z); }
public final static Vector3d sub(Tuple3d v, Tuple3d v1) { return new Vector3d(v.x - v1.x, v.y - v1.y, v.z - v1.z); }
static double dot(Tuple3d v0, Tuple3d v1) { return (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z); }
