diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java
new file mode 100644
index 0000000..0f58bfb
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java
@@ -0,0 +1,133 @@
+/**
+*
+* CoreSLAM.java abstract Java class for CoreSLAM algorithm in BreezySLAM
+*
+* Copyright (C) 2014 Simon D. Levy
+
+* This code is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation, either version 3 of the
+* License, or (at your option) any later version.
+*
+* This code is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this code. If not, see .
+*/
+
+
+package edu.wlu.cs.levy.breezyslam.algorithms;
+
+import edu.wlu.cs.levy.breezyslam.components.Laser;
+import edu.wlu.cs.levy.breezyslam.components.Velocities;
+import edu.wlu.cs.levy.breezyslam.components.Map;
+import edu.wlu.cs.levy.breezyslam.components.Scan;
+
+/**
+* CoreSLAM is an abstract class that uses the classes Position, Map, Scan, and Laser
+* to run variants of the simple CoreSLAM (tinySLAM) algorithm described in
+*
+* @inproceedings{coreslam-2010,
+* author = {Bruno Steux and Oussama El Hamzaoui},
+* title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},
+* booktitle = {11th International Conference on Control, Automation,
+* Robotics and Vision, ICARCV 2010, Singapore, 7-10
+* December 2010, Proceedings},
+* pages = {1975-1979},
+* publisher = {IEEE},
+* year = {2010}
+* }
+*
+* Implementing classes should provide the method
+*
+* void updateMapAndPointcloud(int * scan_mm, Velocities & velocities)
+*
+* to update the map and point-cloud (particle cloud).
+*
+*/
+public abstract class CoreSLAM {
+
+ /**
+ * The quality of the map (0 through 255)
+ */
+ public int map_quality = 50;
+
+ /**
+ * The width in millimeters of each "hole" in the map (essentially, wall width)
+ */
+ public double hole_width_mm = 600;
+
+ protected Laser laser;
+
+ protected Velocities velocities;
+
+ protected Map map;
+
+ protected Scan scan_for_mapbuild;
+ protected Scan scan_for_distance;
+
+ public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters)
+ {
+ // Set default params
+ this.laser = new Laser(laser);
+
+ // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
+ this.velocities = new Velocities();
+
+ // Initialize a scan for computing distance to map, and one for updating map
+ this.scan_for_mapbuild = this.scan_create(3);
+ this.scan_for_distance = this.scan_create(1);
+
+ // Initialize the map
+ this.map = new Map(map_size_pixels, map_size_meters);
+ }
+
+ private Scan scan_create(int span)
+ {
+ return new Scan(this.laser, span);
+ }
+
+ private void scan_update(Scan scan, int [] scan_mm)
+ {
+ scan.update(scan_mm, this.hole_width_mm, this.velocities);
+ }
+
+
+ public void update(int [] scan_mm, Velocities velocities)
+ {
+ // Build a scan for computing distance to map, and one for updating map
+ this.scan_update(this.scan_for_mapbuild, scan_mm);
+ this.scan_update(this.scan_for_distance, scan_mm);
+
+ // Update velocities
+ this.velocities.update(velocities.getDxyMm(), velocities.getDthetaDegrees(), velocities.getDtSeconds());
+
+ // Implementing class updates map and pointcloud
+ this.updateMapAndPointcloud(velocities);
+ }
+
+ /**
+ * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities
+ * (no odometry).
+ * @param scan_mm Lidar scan values, whose count is specified in the scan_size
+ * attribute of the Laser object passed to the CoreSlam constructor
+ */
+ public void update(int [] scan_mm)
+ {
+ Velocities zero_velocities = new Velocities();
+
+ this.update(scan_mm, zero_velocities);
+ }
+
+
+ protected abstract void updateMapAndPointcloud(Velocities velocities);
+
+ public void getmap(byte [] mapbytes)
+ {
+ this.map.get(mapbytes);
+ }
+
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java
new file mode 100644
index 0000000..d0eca8a
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java
@@ -0,0 +1,36 @@
+/**
+* DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching
+* on it; i.e., using odometry alone.
+*/
+
+package edu.wlu.cs.levy.breezyslam.algorithms;
+
+import edu.wlu.cs.levy.breezyslam.components.Position;
+import edu.wlu.cs.levy.breezyslam.components.Laser;
+
+public class DeterministicSLAM extends SinglePositionSLAM
+{
+
+ /**
+ * Creates a DeterministicSLAM object.
+ * @param laser a Laser object containing parameters for your Lidar equipment
+ * @param map_size_pixels the size of the desired map (map is square)
+ * @param map_size_meters the size of the area to be mapped, in meters
+ * @return a new CoreSLAM object
+ */
+ public DeterministicSLAM(Laser laser, int map_size_pixels, double map_size_meters)
+ {
+ super(laser, map_size_pixels, map_size_meters);
+ }
+
+ /**
+ * Returns a new position identical to the starting position. Called automatically by
+ * SinglePositionSLAM::updateMapAndPointcloud()
+ * @param start_pos the starting position
+ */
+ protected Position getNewPosition(Position start_position)
+ {
+ return new Position(start_position.x_mm, start_position.y_mm, start_position.theta_degrees);
+ }
+
+} // DeterministicSLAM
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile
new file mode 100644
index 0000000..745d115
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile
@@ -0,0 +1,73 @@
+BASEDIR = ../../../../../../..
+JAVADIR = $(BASEDIR)/java
+CDIR = $(BASEDIR)/c
+JFLAGS = -Xlint
+JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include
+#JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux
+
+# Set library extension based on OS
+ifeq ("$(shell uname)","Darwin")
+ LIBEXT = dylib
+else ifeq ("$(shell uname)","Linux")
+ CFLAGS = -fPIC
+ LIBEXT = so
+else
+ LIBEXT = dll
+endif
+
+# Set SIMD compile params based on architecture
+ifeq ("$(ARCH)","armv7l")
+ SIMD_FLAGS = -mfpu=neon
+else ifeq ("$(ARCH)","i686")
+ SIMD_FLAGS = -msse3
+else
+ ARCH = sisd
+endif
+
+
+ALL = libjnibreezyslam_algorithms.$(LIBEXT) CoreSLAM.class SinglePositionSLAM.class DeterministicSLAM.class RMHCSLAM.class
+
+all: $(ALL)
+
+libjnibreezyslam_algorithms.$(LIBEXT): jnibreezyslam_algorithms.o coreslam.o random.o ziggurat.o coreslam_$(ARCH).o
+ gcc -shared -Wl,-soname,libjnibreezyslam_algorithms.so -o libjnibreezyslam_algorithms.so jnibreezyslam_algorithms.o \
+ coreslam.o coreslam_$(ARCH).o random.o ziggurat.o
+
+jnibreezyslam_algorithms.o: jnibreezyslam_algorithms.c RMHCSLAM.h ../jni_utils.h
+ gcc $(JDKINC) -fPIC -c jnibreezyslam_algorithms.c
+
+
+CoreSLAM.class: CoreSLAM.java
+ javac -classpath $(JAVADIR):. CoreSLAM.java
+
+
+SinglePositionSLAM.class: SinglePositionSLAM.java
+ javac -classpath $(JAVADIR):. SinglePositionSLAM.java
+
+DeterministicSLAM.class: DeterministicSLAM.java
+ javac -classpath $(JAVADIR):. DeterministicSLAM.java
+
+RMHCSLAM.class: RMHCSLAM.java
+ javac -classpath $(JAVADIR):. RMHCSLAM.java
+
+RMHCSLAM.h: RMHCSLAM.class
+ javah -o RMHCSLAM.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM
+
+coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h
+ gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c
+
+coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h
+ gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c
+
+random.o: $(CDIR)/random.c
+ gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/random.c
+
+ziggurat.o: $(CDIR)/ziggurat.c
+ gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/ziggurat.c
+
+clean:
+ rm -f *.class *.o *.h *.$(LIBEXT) *~
+
+backup:
+ cp *.java bak
+ cp Makefile bak
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java
new file mode 100644
index 0000000..62b5815
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java
@@ -0,0 +1,88 @@
+/**
+* RMHCSLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position.
+*/
+
+package edu.wlu.cs.levy.breezyslam.algorithms;
+
+import edu.wlu.cs.levy.breezyslam.components.Position;
+import edu.wlu.cs.levy.breezyslam.components.Laser;
+import edu.wlu.cs.levy.breezyslam.components.Velocities;
+import edu.wlu.cs.levy.breezyslam.components.Map;
+import edu.wlu.cs.levy.breezyslam.components.Scan;
+
+public class RMHCSLAM extends SinglePositionSLAM
+{
+ private static final double DEFAULT_SIGMA_XY_MM = 100;
+ private static final double DEFAULT_SIGMA_THETA_DEGREES = 20;
+ private static final int DEFAULT_MAX_SEARCH_ITER = 1000;
+
+ static
+ {
+ System.loadLibrary("jnibreezyslam_algorithms");
+ }
+
+ private native void init(int random_seed);
+
+ private native Object positionSearch(
+ Position start_pos,
+ Map map,
+ Scan scan,
+ double sigma_xy_mm,
+ double sigma_theta_degrees,
+ int max_search_iter);
+
+
+ private long native_ptr;
+
+ /**
+ * Creates an RMHCSLAM object.
+ * @param laser a Laser object containing parameters for your Lidar equipment
+ * @param map_size_pixels the size of the desired map (map is square)
+ * @param map_size_meters the size of the area to be mapped, in meters
+ * @param random_seed seed for psuedorandom number generator in particle filter
+ * @return a new CoreSLAM object
+ */
+ public RMHCSLAM(Laser laser, int map_size_pixels, double map_size_meters, int random_seed)
+ {
+ super(laser, map_size_pixels, map_size_meters);
+
+ this.init(random_seed);
+ }
+
+ /**
+ * The standard deviation in millimeters of the Gaussian distribution of
+ * the (X,Y) component of position in the particle filter; default = 100
+ */
+ public double sigma_xy_mm = DEFAULT_SIGMA_XY_MM;;
+
+ /**
+ * The standard deviation in degrees of the Gaussian distribution of
+ * the angular rotation component of position in the particle filter; default = 20
+ */
+ public double sigma_theta_degrees = DEFAULT_SIGMA_THETA_DEGREES;
+
+ /**
+ * The maximum number of iterations for particle-filter search; default = 1000
+ */
+ public int max_search_iter = DEFAULT_MAX_SEARCH_ITER;
+
+ /**
+ * Returns a new position based on RMHC search from a starting position. Called automatically by
+ * SinglePositionSLAM::updateMapAndPointcloud()
+ * @param start_position the starting position
+ */
+ protected Position getNewPosition(Position start_position)
+ {
+ Position newpos =
+ (Position)positionSearch(
+ start_position,
+ this.map,
+ this.scan_for_distance,
+ this.sigma_xy_mm,
+ this.sigma_theta_degrees,
+ this.max_search_iter);
+
+ return newpos;
+ }
+
+} // RMHCSLAM
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java
new file mode 100644
index 0000000..80b0924
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java
@@ -0,0 +1,103 @@
+/**
+* SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud
+* with a single point (particle, position). Implementing classes should provide the method
+*
+* Position getNewPosition(Position start_position)
+*
+* to compute a new position based on searching from a starting position.
+*/
+
+package edu.wlu.cs.levy.breezyslam.algorithms;
+
+import edu.wlu.cs.levy.breezyslam.components.Position;
+import edu.wlu.cs.levy.breezyslam.components.Velocities;
+import edu.wlu.cs.levy.breezyslam.components.Laser;
+
+
+public abstract class SinglePositionSLAM extends CoreSLAM
+{
+ /**
+ * Returns the current position.
+ * @return the current position as a Position object.
+ */
+ public Position getpos()
+ {
+ return this.position;
+ }
+
+ /**
+ * Creates a SinglePositionSLAM object.
+ * @param laser a Laser object containing parameters for your Lidar equipment
+ * @param map_size_pixels the size of the desired map (map is square)
+ * @param map_size_meters the size of the area to be mapped, in meters
+ * @return a new SinglePositionSLAM object
+ */
+ protected SinglePositionSLAM(Laser laser, int map_size_pixels, double map_size_meters)
+ {
+ super(laser, map_size_pixels, map_size_meters);
+
+ this.position = new Position(this.init_coord_mm(), this.init_coord_mm(), 0);
+ }
+
+
+ /**
+ * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update()
+ * @param velocities velocities for odometry
+ */
+ protected void updateMapAndPointcloud(Velocities velocities)
+ {
+ // Start at current position
+ Position start_pos = new Position(this.position);
+
+ // Add effect of velocities
+ start_pos.x_mm += velocities.getDxyMm() * this.costheta();
+ start_pos.y_mm += velocities.getDxyMm() * this.sintheta();
+ start_pos.theta_degrees += velocities.getDthetaDegrees();
+
+ // Add offset from laser
+ start_pos.x_mm += this.laser.getOffsetMm() * this.costheta();
+ start_pos.y_mm += this.laser.getOffsetMm() * this.sintheta();
+
+ // Get new position from implementing class
+ Position new_position = this.getNewPosition(start_pos);
+
+ // Update the map with this new position
+ this.map.update(this.scan_for_mapbuild, new_position, this.map_quality, this.hole_width_mm);
+
+ // Update the current position with this new position, adjusted by laser offset
+ this.position = new Position(new_position);
+ this.position.x_mm -= this.laser.getOffsetMm() * this.costheta();
+ this.position.y_mm -= this.laser.getOffsetMm() * this.sintheta();
+ }
+
+ /**
+ * Returns a new position based on searching from a starting position. Called automatically by
+ * SinglePositionSLAM::updateMapAndPointcloud()
+ * @param start_pos the starting position
+ */
+
+ protected abstract Position getNewPosition(Position start_pos);
+
+ private Position position;
+
+ private double theta_radians()
+ {
+ return java.lang.Math.toRadians(this.position.theta_degrees);
+ }
+
+ private double costheta()
+ {
+ return java.lang.Math.cos(this.theta_radians());
+ }
+
+ private double sintheta()
+ {
+ return java.lang.Math.sin(this.theta_radians());
+ }
+
+ private double init_coord_mm()
+ {
+ // Center of map
+ return 500 * this.map.sizeMeters();
+ }
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java
new file mode 100644
index 0000000..b8481d4
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java
@@ -0,0 +1,66 @@
+package edu.wlu.cs.levy.breezyslam.algorithms;
+
+import edu.wlu.cs.levy.breezyslam.components.Laser;
+import edu.wlu.cs.levy.breezyslam.components.Velocities;
+
+public class CoreSLAM {
+
+ public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters)
+ {
+ /*
+ // Set default params
+ this->map_quality = DEFAULT_MAP_QUALITY;
+ this->hole_width_mm = DEFAULT_HOLE_WIDTH_MM;
+
+ // Store laser for later
+ this->laser = new Laser(laser);
+
+ // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
+ this->velocities = new Velocities();
+
+ // Initialize a scan for computing distance to map, and one for updating map
+ this->scan_for_mapbuild = this->scan_create(3);
+ this->scan_for_distance = this->scan_create(1);
+
+ // Initialize the map
+ this->map = new Map(map_size_pixels, map_size_meters);
+ */
+ }
+
+
+ public void update(int [] scan_mm, Velocities velocities)
+ {
+ /*
+ // Build a scan for computing distance to map, and one for updating map
+ this->scan_update(this->scan_for_mapbuild, scan_mm);
+ this->scan_update(this->scan_for_distance, scan_mm);
+
+ // Update velocities
+ this->velocities->update(velocities.dxy_mm,
+ velocities.dtheta_degrees,
+ velocities.dt_seconds);
+
+ // Implementing class updates map and pointcloud
+ this->updateMapAndPointcloud(velocities);
+ */
+ }
+
+
+ public void getmap(byte [] mapbytes)
+ {
+ //this->map->get((char *)mapbytes);
+ }
+
+ /*
+ Scan * CoreSLAM::scan_create(int span)
+ {
+ //return new Scan(this->laser, span);
+ }
+
+
+ void scan_update(Scan scan, int [] scan_mm)
+ {
+ //scan->update(scan_mm, this->hole_width_mm, *this->velocities);
+ }
+ */
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile
new file mode 100644
index 0000000..8918ed4
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile
@@ -0,0 +1,15 @@
+JAVADIR = ../../../../../..
+
+ALL = CoreSLAM.class
+
+all: $(ALL)
+
+CoreSLAM.class: CoreSLAM.java
+ javac -classpath $(JAVADIR):. CoreSLAM.java
+
+clean:
+ rm -f *.class *~
+
+backup:
+ cp *.java bak
+ cp Makefile bak
diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c
new file mode 100644
index 0000000..db97d3d
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c
@@ -0,0 +1,47 @@
+#include "../../../../../../../c/random.h"
+#include "../jni_utils.h"
+
+#include
+
+
+// RMHC_SLAM methods -----------------------------------------------------------------------------------------
+
+JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_init (JNIEnv *env, jobject thisobject, jint random_seed)
+{
+ ptr_to_obj(env, thisobject, random_new(random_seed));
+}
+
+JNIEXPORT jobject JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_positionSearch (JNIEnv *env, jobject thisobject,
+ jobject startpos_object,
+ jobject map_object,
+ jobject scan_object,
+ jdouble sigma_xy_mm,
+ jdouble sigma_theta_degrees,
+ jint max_search_iter)
+{
+ position_t startpos;
+
+ startpos.x_mm = get_double_field(env, startpos_object, "x_mm");
+ startpos.y_mm = get_double_field(env, startpos_object, "y_mm");
+ startpos.theta_degrees = get_double_field(env, startpos_object, "theta_degrees");
+
+ void * random = ptr_from_obj(env, thisobject);
+
+ position_t newpos =
+ rmhc_position_search(
+ startpos,
+ cmap_from_jmap(env, map_object),
+ cscan_from_jscan(env, scan_object),
+ sigma_xy_mm,
+ sigma_theta_degrees,
+ max_search_iter,
+ random);
+
+ jclass cls = (*env)->FindClass(env, "edu/wlu/cs/levy/breezyslam/components/Position");
+
+ jmethodID constructor = (*env)->GetMethodID(env, cls, "", "(DDD)V");
+
+ jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees);
+
+ return newpos_object;
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Laser.java b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java
new file mode 100644
index 0000000..d1f4b23
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java
@@ -0,0 +1,58 @@
+package edu.wlu.cs.levy.breezyslam.components;
+
+public class Laser
+{
+
+ protected int scan_size;
+ protected double scan_rate_hz;
+ protected double detection_angle_degrees;
+ protected double distance_no_detection_mm;
+ protected int detection_margin;
+ protected double offset_mm;
+
+ public Laser(
+ int scan_size,
+ double scan_rate_hz,
+ double detection_angle_degrees,
+ double distance_no_detection_mm,
+ int detection_margin,
+ double offset_mm)
+ {
+ this.scan_size = scan_size;
+ this.scan_rate_hz = scan_rate_hz;
+ this.detection_angle_degrees = detection_angle_degrees;
+ this.distance_no_detection_mm = distance_no_detection_mm;
+ this.detection_margin = detection_margin;
+ this.offset_mm = offset_mm;
+ }
+
+ public Laser(Laser laser)
+ {
+ this.scan_size = laser.scan_size;
+ this.scan_rate_hz = laser.scan_rate_hz;
+ this.detection_angle_degrees = laser.detection_angle_degrees;
+ this.distance_no_detection_mm = laser.distance_no_detection_mm;
+ this.detection_margin = laser.detection_margin;
+ this.offset_mm = laser.offset_mm;
+ }
+
+ public String toString()
+ {
+ String format = "scan_size=%d | scan_rate=%3.3f hz | " +
+ "detection_angle=%3.3f deg | " +
+ "distance_no_detection=%7.4f mm | " +
+ "detection_margin=%d | offset=%4.4f m";
+
+ return String.format(format, this.scan_size, this.scan_rate_hz,
+ this.detection_angle_degrees,
+ this.distance_no_detection_mm,
+ this.detection_margin,
+ this.offset_mm);
+
+ }
+
+ public double getOffsetMm()
+ {
+ return this.offset_mm;
+ }
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Makefile b/java/edu/wlu/cs/levy/breezyslam/components/Makefile
new file mode 100644
index 0000000..d037b4c
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/Makefile
@@ -0,0 +1,72 @@
+BASEDIR = ../../../../../../..
+JAVADIR = $(BASEDIR)/java
+CDIR = $(BASEDIR)/c
+JFLAGS = -Xlint
+JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include
+#JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux
+
+# Set library extension based on OS
+ifeq ("$(shell uname)","Darwin")
+ LIBEXT = dylib
+else ifeq ("$(shell uname)","Linux")
+ CFLAGS = -fPIC
+ LIBEXT = so
+else
+ LIBEXT = dll
+endif
+
+ARCH = $(shell uname -m)
+
+# Set SIMD compile params based on architecture
+ifeq ("$(ARCH)","armv7l")
+ SIMD_FLAGS = -mfpu=neon
+else ifeq ("$(ARCH)","i686")
+ SIMD_FLAGS = -msse3
+else
+ ARCH = sisd
+endif
+
+ALL = libjnibreezyslam_components.$(LIBEXT) Laser.class Position.class Velocities.class URG04LX.class
+
+all: $(ALL)
+
+libjnibreezyslam_components.$(LIBEXT): jnibreezyslam_components.o coreslam.o coreslam_$(ARCH).o
+ gcc -shared -Wl,-soname,libjnibreezyslam_components.so -o libjnibreezyslam_components.so jnibreezyslam_components.o \
+ coreslam.o coreslam_$(ARCH).o
+
+jnibreezyslam_components.o: jnibreezyslam_components.c Map.h Scan.h ../jni_utils.h
+ gcc $(JDKINC) -fPIC -c jnibreezyslam_components.c
+
+coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h
+ gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c
+
+coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h
+ gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c
+
+Map.h: Map.class
+ javah -o Map.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Map
+
+Map.class: Map.java Scan.class Position.class
+ javac $(JFLAGS) -classpath $(JAVADIR) Map.java
+
+Scan.h: Scan.class
+ javah -o Scan.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Scan
+
+Scan.class: Scan.java Laser.class
+ javac $(JFLAGS) -classpath $(JAVADIR) Scan.java
+
+Laser.class: Laser.java
+ javac $(JFLAGS) Laser.java
+
+URG04LX.class: URG04LX.java Laser.class
+ javac $(JFLAGS) -classpath $(JAVADIR) URG04LX.java
+
+Velocities.class: Velocities.java
+ javac $(JFLAGS) Velocities.java
+
+
+Position.class: Position.java
+ javac $(JFLAGS) Position.java
+
+clean:
+ rm -f *.so *.class *.o *.h *~
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Map.java b/java/edu/wlu/cs/levy/breezyslam/components/Map.java
new file mode 100644
index 0000000..22e1da8
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/Map.java
@@ -0,0 +1,58 @@
+package edu.wlu.cs.levy.breezyslam.components;
+
+public class Map
+{
+ static
+ {
+ System.loadLibrary("jnibreezyslam_components");
+ }
+
+ private native void init(int size_pixels, double size_meters);
+
+ private double size_meters;
+
+ private native void update(
+ Scan scan,
+ double position_x_mm,
+ double position_y_mm,
+ double position_theta_degrees,
+ int quality,
+ double hole_width_mm);
+
+ private long native_ptr;
+
+ public native String toString();
+
+ public Map(int size_pixels, double size_meters)
+ {
+ this.init(size_pixels, size_meters);
+
+ // for public accessor
+ this.size_meters = size_meters;
+ }
+
+ /**
+ * Puts current map values into bytearray, which should of which should be of
+ * this->size map_size_pixels ^ 2.
+ */
+ public native void get(byte [] bytes);
+
+ /**
+ * Updates this map object based on new data.
+ * @param scan a new scan
+ * @param position a new postion
+ * @param quality speed with which scan is integerate into map (0 through 255)
+ * @param hole_width_mm hole width in millimeters
+ *
+ */
+ public void update(Scan scan, Position position, int quality, double hole_width_mm)
+ {
+ this.update(scan, position.x_mm, position.y_mm, position.theta_degrees, quality, hole_width_mm);
+ }
+
+ public double sizeMeters()
+ {
+ return this.size_meters;
+ }
+
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Position.java b/java/edu/wlu/cs/levy/breezyslam/components/Position.java
new file mode 100644
index 0000000..862a55e
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/Position.java
@@ -0,0 +1,38 @@
+package edu.wlu.cs.levy.breezyslam.components;
+
+public class Position
+{
+
+ public double x_mm;
+ public double y_mm;
+ public double theta_degrees;
+
+ public Position(double x_mm, double y_mm, double theta_degrees)
+ {
+ this.x_mm = x_mm;
+ this.y_mm = y_mm;
+ this.theta_degrees = theta_degrees;
+ }
+
+ public Position(Position position)
+ {
+ this.x_mm = position.x_mm;
+ this.y_mm = position.y_mm;
+ this.theta_degrees = position.theta_degrees;
+ }
+
+ public String toString()
+ {
+ //String format = "";
+ String format = "";
+
+ return String.format(format, this.x_mm, this.y_mm, this.theta_degrees);
+
+ }
+
+ public static void main(String[] argv)
+ {
+ Position position = new Position(300, 400, 120);
+ System.out.println(position);
+ }
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java
new file mode 100644
index 0000000..b14a3dc
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java
@@ -0,0 +1,58 @@
+package edu.wlu.cs.levy.breezyslam.components;
+
+public class Scan
+{
+ static
+ {
+ System.loadLibrary("jnibreezyslam_components");
+ }
+
+ private native void init(
+ int span,
+ int scan_size,
+ double scan_rate_hz,
+ double detection_angle_degrees,
+ double distance_no_detection_mm,
+ int detection_margin,
+ double offset_mm);
+
+ private long native_ptr;
+
+ public native String toString();
+
+ public native void update(
+ int [] lidar_mm,
+ double hole_width_mm,
+ double velocities_dxy_mm,
+ double velocities_dtheta_degrees);
+
+
+ public Scan(Laser laser, int span)
+ {
+ this.init(span,
+ laser.scan_size,
+ laser.scan_rate_hz,
+ laser.detection_angle_degrees,
+ laser.distance_no_detection_mm,
+ laser.detection_margin,
+ laser.offset_mm);
+ }
+
+ public Scan(Laser laser)
+ {
+ this(laser, 1);
+ }
+
+ /**
+ * Updates this Scan object with new values from a Lidar scan.
+ * @param scanvals_mm scanned Lidar distance values in millimeters
+ * @param hole_width_millimeters hole width in millimeters
+ * @param velocities forward velocity and angular velocity of robot at scan time
+ *
+ */
+ public void update(int [] scanvals_mm, double hole_width_millimeters, Velocities velocities)
+ {
+ this.update(scanvals_mm, hole_width_millimeters, velocities.dxy_mm, velocities.dtheta_degrees);
+ }
+}
+
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java
new file mode 100644
index 0000000..4696d47
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java
@@ -0,0 +1,15 @@
+package edu.wlu.cs.levy.breezyslam.components;
+
+public class URG04LX extends Laser
+{
+
+ public URG04LX(int detection_margin, double offset_mm)
+ {
+ super(682, 10, 240, 4000, detection_margin, offset_mm);
+ }
+
+ public URG04LX()
+ {
+ this(0, 0);
+ }
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java b/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java
new file mode 100644
index 0000000..76e2855
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java
@@ -0,0 +1,68 @@
+package edu.wlu.cs.levy.breezyslam.components;
+
+/**
+* A class representing the forward and angular velocities of a robot.
+*/
+public class Velocities
+{
+
+ /**
+ * Creates a new Velocities object with specified velocities.
+ */
+ public Velocities(double dxy_mm, double dtheta_degrees, double dtSeconds)
+ {
+ this.dxy_mm = dxy_mm;
+ this.dtheta_degrees = dtheta_degrees;
+ this.dt_seconds = dtSeconds;
+ }
+
+ /**
+ * Creates a new Velocities object with zero velocities.
+ */
+ public Velocities()
+ {
+ this.dxy_mm = 0;
+ this.dtheta_degrees = 0;
+ this.dt_seconds = 0;
+ }
+
+ /**
+ * Updates this Velocities object.
+ * @param dxy_mm new forward distance traveled in millimeters
+ * @param dtheta_degrees new angular rotation in degrees
+ * @param dtSeconds time in seconds since last velocities
+ */
+ public void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
+ {
+ double velocity_factor = (dtSeconds > 0) ? (1 / dtSeconds) : 0;
+
+ this.dxy_mm = dxy_mm * velocity_factor;
+
+ this.dtheta_degrees = dtheta_degrees * velocity_factor;
+ }
+
+ public String toString()
+ {
+ return String.format("
+#include
+
+#define MAXSTR 100
+
+// Map methods -----------------------------------------------------------------------------------------
+
+JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_init (JNIEnv *env, jobject thisobject, jint size_pixels, jdouble size_meters)
+{
+ map_t * map = (map_t *)malloc(sizeof(map_t));
+ map_init(map, (int)size_pixels, (double)size_meters);
+ ptr_to_obj(env, thisobject, map);
+}
+
+JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_toString (JNIEnv *env, jobject thisobject)
+{
+ map_t * map = cmap_from_jmap(env, thisobject);
+
+ char str[MAXSTR];
+
+ map_string(*map, str);
+
+ return (*env)->NewStringUTF(env, str);
+}
+
+JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_get (JNIEnv *env, jobject thisobject, jbyteArray bytes)
+{
+ map_t * map = cmap_from_jmap(env, thisobject);
+
+ jbyte * ptr = (*env)->GetByteArrayElements(env, bytes, NULL);
+
+ map_get(map, ptr);
+
+ (*env)->ReleaseByteArrayElements(env, bytes, ptr, 0);
+}
+
+JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_update (JNIEnv *env, jobject thisobject,
+ jobject scanobject,
+ jdouble position_x_mm,
+ jdouble position_y_mm,
+ jdouble position_theta_degrees,
+ jint quality,
+ jdouble hole_width_mm)
+{
+ map_t * map = cmap_from_jmap(env, thisobject);
+
+ scan_t * scan = cscan_from_jscan(env, scanobject);
+
+ position_t position;
+
+ position.x_mm = position_x_mm;
+ position.y_mm = position_y_mm;
+ position.theta_degrees = position_theta_degrees;
+
+ map_update(map, scan, position, quality, hole_width_mm);
+}
+
+
+// Scan methods -----------------------------------------------------------------------------------------
+
+JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_init (JNIEnv *env, jobject thisobject,
+ jint span,
+ jint scan_size,
+ jdouble scan_rate_hz,
+ jdouble detection_angle_degrees,
+ jdouble distance_no_detection_mm,
+ jint detection_margin,
+ jdouble offset_mm)
+ {
+ scan_t * scan = (scan_t *)malloc(sizeof(scan_t));
+
+ scan_init(scan,
+ span,
+ scan_size,
+ scan_rate_hz,
+ detection_angle_degrees,
+ distance_no_detection_mm,
+ detection_margin,
+ offset_mm);
+
+ ptr_to_obj(env, thisobject, scan);
+}
+
+JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_toString (JNIEnv *env, jobject thisobject)
+{
+ scan_t * scan = cscan_from_jscan(env, thisobject);
+
+ char str[MAXSTR];
+
+ scan_string(*scan, str);
+
+ return (*env)->NewStringUTF(env, str);
+}
+
+JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_update (JNIEnv *env, jobject thisobject,
+ jintArray lidar_mm,
+ jdouble hole_width_mm,
+ jdouble velocities_dxy_mm,
+ jdouble velocities_dtheta_degrees)
+{
+ scan_t * scan = cscan_from_jscan(env, thisobject);
+
+ jint * lidar_mm_c = (*env)->GetIntArrayElements(env, lidar_mm, 0);
+
+ scan_update(scan, lidar_mm_c, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees);
+
+ (*env)->ReleaseIntArrayElements(env, lidar_mm, lidar_mm_c, 0);
+}
diff --git a/java/edu/wlu/cs/levy/breezyslam/jni_utils.h b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h
new file mode 100644
index 0000000..079875a
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h
@@ -0,0 +1,42 @@
+#include "../../../../../../c/coreslam.h"
+
+#include
+#include
+
+static jfieldID get_fid(JNIEnv *env, jobject object, const char * fieldname, const char * fieldsig)
+{
+ jclass cls = (*env)->GetObjectClass(env, object);
+ return (*env)->GetFieldID(env, cls, fieldname, fieldsig);
+}
+
+static jfieldID get_this_fid(JNIEnv *env, jobject thisobject)
+{
+ return get_fid(env, thisobject, "native_ptr", "J");
+}
+
+static void * ptr_from_obj(JNIEnv *env, jobject thisobject)
+{
+ return (void *)(*env)->GetLongField (env, thisobject, get_this_fid(env, thisobject));
+}
+
+static void ptr_to_obj(JNIEnv *env, jobject thisobject, void * ptr)
+{
+ (*env)->SetLongField (env, thisobject, get_this_fid(env, thisobject), (long)ptr);
+}
+
+static scan_t * cscan_from_jscan(JNIEnv *env, jobject thisobject)
+{
+ return (scan_t *)ptr_from_obj(env, thisobject);
+}
+
+static map_t * cmap_from_jmap(JNIEnv *env, jobject thisobject)
+{
+ return (map_t *)ptr_from_obj(env, thisobject);
+}
+
+static double get_double_field(JNIEnv *env, jobject object, const char * fieldname)
+{
+ return (*env)->GetDoubleField (env, object, get_fid(env, object, fieldname, "D"));
+}
+
+
diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/Makefile b/java/edu/wlu/cs/levy/breezyslam/robots/Makefile
new file mode 100644
index 0000000..f97b0d9
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/robots/Makefile
@@ -0,0 +1,13 @@
+BASEDIR = ../../../../../../..
+JAVADIR = $(BASEDIR)/java
+JFLAGS = -Xlint
+
+ALL = WheeledRobot.class
+
+all: $(ALL)
+
+WheeledRobot.class: WheeledRobot.java
+ javac $(JFLAGS) -classpath $(JAVADIR) WheeledRobot.java
+
+clean:
+ rm -f *.class *~
diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java
new file mode 100644
index 0000000..8ccc871
--- /dev/null
+++ b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java
@@ -0,0 +1,130 @@
+/**
+ *
+ * BreezySLAM: Simple, efficient SLAM in C++
+ *
+ * WheeledRobot.java - Java class for wheeled robots
+ *
+ * Copyright (C) 2014 Simon D. Levy
+
+ * This code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation, either version 3 of the
+ * License, or (at your option) any later version.
+ *
+ * This code is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this code. If not, see .
+ */
+
+
+package edu.wlu.cs.levy.breezyslam.robots;
+
+import edu.wlu.cs.levy.breezyslam.components.Velocities;
+
+
+/**
+ * An abstract class for wheeled robots. Supports computation of forward and angular
+ * velocities based on odometry. Your subclass should should implement the
+ * extractOdometry method.
+ */
+public abstract class WheeledRobot
+{
+
+ /**
+ * Builds a WheeledRobot object. Parameters should be based on the specifications for
+ * your robot.
+ * @param wheel_radius_mm radius of each odometry wheel, in meters
+ * @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters
+ * @return a new WheeledRobot object
+ */
+ protected WheeledRobot(double wheel_radius_mm, double half_axle_length_mm)
+ {
+ this.wheel_radius_mm = wheel_radius_mm;
+ this.half_axle_length_mm = half_axle_length_mm;
+
+ this.timestamp_seconds_prev = 0;
+ this.left_wheel_degrees_prev = 0;
+ this.right_wheel_degrees_prev = 0;
+ }
+
+ public String toString()
+ {
+
+ return String.format("",
+ this.wheel_radius_mm, this.half_axle_length_mm, this.descriptorString());
+ }
+
+ /**
+ * Computes forward and angular velocities based on odometry.
+ * @param timestamp time stamp, in whatever units your robot uses
+ * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses
+ * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
+ * @return velocities object representing velocities for these odometry values
+ */
+
+ public Velocities computeVelocities( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
+ {
+ WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry);
+
+ double dxy_mm = 0;
+ double dtheta_degrees = 0;
+ double dt_seconds = 0;
+
+ if (this.timestamp_seconds_prev > 0)
+ {
+ double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev;
+ double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev;
+
+ dxy_mm = this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees));
+
+ dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees);
+
+ dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev;
+ }
+
+ // Store current odometry for next time
+ this.timestamp_seconds_prev = odometry.timestamp_seconds;
+ this.left_wheel_degrees_prev = odometry.left_wheel_degrees;
+ this.right_wheel_degrees_prev = odometry.right_wheel_degrees;
+
+ return new Velocities(dxy_mm, dtheta_degrees, dt_seconds);
+ }
+
+ /**
+ * Extracts usable odometry values from your robot's odometry.
+ * @param timestamp time stamp, in whatever units your robot uses
+ * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses
+ * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
+ * @return WheelOdometry object containing timestamp in seconds, left wheel degrees, right wheel degrees
+ */
+ protected abstract WheelOdometry extractOdometry(double timestamp, double left_wheel_odometry, double right_wheel_odometry);
+
+ protected abstract String descriptorString();
+
+ protected class WheelOdometry
+ {
+ public WheelOdometry(double timestamp_seconds, double left_wheel_degrees, double right_wheel_degrees)
+ {
+ this.timestamp_seconds = timestamp_seconds;
+ this.left_wheel_degrees = left_wheel_degrees;
+ this.right_wheel_degrees = right_wheel_degrees;
+ }
+
+ public double timestamp_seconds;
+ public double left_wheel_degrees;
+ public double right_wheel_degrees;
+
+ }
+
+
+ private double wheel_radius_mm;
+ private double half_axle_length_mm;
+
+ private double timestamp_seconds_prev;
+ private double left_wheel_degrees_prev;
+ private double right_wheel_degrees_prev;
+}