diff --git a/wpilibc/shared/include/CircularBuffer.h b/wpilibc/shared/include/CircularBuffer.h index 872461d321..e5d123fb68 100644 --- a/wpilibc/shared/include/CircularBuffer.h +++ b/wpilibc/shared/include/CircularBuffer.h @@ -23,6 +23,7 @@ class CircularBuffer { void PushBack(T value); T PopFront(); T PopBack(); + void Resize(size_t size); void Reset(); T& operator[](size_t index); diff --git a/wpilibc/shared/include/CircularBuffer.inc b/wpilibc/shared/include/CircularBuffer.inc index e6f9ceb983..86ac7e1b68 100644 --- a/wpilibc/shared/include/CircularBuffer.inc +++ b/wpilibc/shared/include/CircularBuffer.inc @@ -81,6 +81,59 @@ T CircularBuffer::PopBack() { return m_data[(m_front + m_length) % m_data.size()]; } +/** + * Resizes internal buffer to given size. + */ +template +void CircularBuffer::Resize(size_t size) { + if (size > m_data.size()) { + // Find end of buffer + size_t insertLocation = (m_front + m_length) % m_data.size(); + + // If insertion location precedes front of buffer, push front index back + if (insertLocation <= m_front) { + m_front += size - m_data.size(); + } + + // Add elements to end of buffer + m_data.insert(m_data.begin() + insertLocation, size - m_data.size(), 0); + } else if (size < m_data.size()) { + /* 1) Shift element block start at "front" left as many blocks as were + * removed up to but not exceeding buffer[0] + * 2) Shrink buffer, which will remove even more elements automatically if + * necessary + */ + size_t elemsToRemove = m_data.size() - size; + auto frontIter = m_data.begin() + m_front; + if (m_front < elemsToRemove) { + /* Remove elements from end of buffer before shifting start of element + * block. Doing so saves a few copies. + */ + m_data.erase(frontIter + size, m_data.end()); + + // Shift start of element block to left + m_data.erase(m_data.begin(), frontIter); + + // Update metadata + m_front = 0; + } else { + // Shift start of element block to left + m_data.erase(frontIter - elemsToRemove, frontIter); + + // Update metadata + m_front -= elemsToRemove; + } + + /* Length only changes during a shrink if all unused spaces have been + * removed. Length decreases as used spaces are removed to meet the + * required size. + */ + if (m_length > size) { + m_length = size; + } + } +} + /** * Sets internal buffer contents to zero. */ diff --git a/wpilibcIntegrationTests/src/CircularBufferTest.cpp b/wpilibcIntegrationTests/src/CircularBufferTest.cpp index bfb2f387bc..dbc7927c3a 100644 --- a/wpilibcIntegrationTests/src/CircularBufferTest.cpp +++ b/wpilibcIntegrationTests/src/CircularBufferTest.cpp @@ -87,3 +87,123 @@ TEST(CircularBufferTest, PushPopTest) { // Leaving only one element with value == 4 EXPECT_EQ(4.0, queue[0]); } + +TEST(CircularBufferTest, ResetTest) { + CircularBuffer queue(5); + + for (size_t i = 1; i < 6; i++) { + queue.PushBack(i); + } + + queue.Reset(); + + for (size_t i = 0; i < 5; i++) { + EXPECT_EQ(0.0, queue[i]); + } +} + +TEST(CircularBufferTest, ResizeTest) { + CircularBuffer queue(5); + + /* Buffer contains {1, 2, 3, _, _} + * ^ front + */ + queue.PushBack(1.0); + queue.PushBack(2.0); + queue.PushBack(3.0); + + queue.Resize(2); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Resize(5); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Reset(); + + /* Buffer contains {_, 1, 2, 3, _} + * ^ front + */ + queue.PushBack(0.0); + queue.PushBack(1.0); + queue.PushBack(2.0); + queue.PushBack(3.0); + queue.PopFront(); + + queue.Resize(2); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Resize(5); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Reset(); + + /* Buffer contains {_, _, 1, 2, 3} + * ^ front + */ + queue.PushBack(0.0); + queue.PushBack(0.0); + queue.PushBack(1.0); + queue.PushBack(2.0); + queue.PushBack(3.0); + queue.PopFront(); + queue.PopFront(); + + queue.Resize(2); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Resize(5); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Reset(); + + /* Buffer contains {3, _, _, 1, 2} + * ^ front + */ + queue.PushBack(3.0); + queue.PushFront(2.0); + queue.PushFront(1.0); + + queue.Resize(2); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Resize(5); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Reset(); + + /* Buffer contains {2, 3, _, _, 1} + * ^ front + */ + queue.PushBack(2.0); + queue.PushBack(3.0); + queue.PushFront(1.0); + + queue.Resize(2); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + queue.Resize(5); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + + // Test PushBack() after resize + queue.PushBack(3.0); + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + EXPECT_EQ(3.0, queue[2]); + + // Test PushFront() after resize + queue.PushFront(4.0); + EXPECT_EQ(4.0, queue[0]); + EXPECT_EQ(1.0, queue[1]); + EXPECT_EQ(2.0, queue[2]); + EXPECT_EQ(3.0, queue[3]); +} diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java index 31e6626886..2670afb5a3 100644 --- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/CircularBuffer.java @@ -97,12 +97,27 @@ public class CircularBuffer { return m_data[(m_front + m_length) % m_data.length]; } + /** + * Resizes internal buffer to given size. + * + *

A new buffer is allocated because arrays are not resizable. + */ + void resize(int size) { + double[] newBuffer = new double[size]; + m_length = Math.min(m_length, size); + for (int i = 0; i < m_length; i++) { + newBuffer[i] = m_data[(m_front + i) % m_data.length]; + } + m_data = newBuffer; + m_front = 0; + } + /** * Sets internal buffer contents to zero. */ public void reset() { - for (double i : m_data) { - i = 0.0; + for (int i = 0; i < m_data.length; i++) { + m_data[i] = 0.0; } m_front = 0; m_length = 0; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java index b93b2443ba..74e80ed4cd 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/CircularBufferTest.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ +/* Copyright (c) FIRST 2015-2016. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -94,4 +94,126 @@ public class CircularBufferTest { // Leaving only one element with value == 4 assertEquals(4.0, queue.get(0), 0.00005); } + + @Test + public void resetTest() { + CircularBuffer queue = new CircularBuffer(5); + + for (int i = 0; i < 6; i++) { + queue.pushBack(i); + } + + queue.reset(); + + for (int i = 0; i < 5; i++) { + assertEquals(0.0, queue.get(i), 0.00005); + } + } + + @Test + public void resizeTest() { + CircularBuffer queue = new CircularBuffer(5); + + /* Buffer contains {1, 2, 3, _, _} + * ^ front + */ + queue.pushBack(1.0); + queue.pushBack(2.0); + queue.pushBack(3.0); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.reset(); + + /* Buffer contains {_, 1, 2, 3, _} + * ^ front + */ + queue.pushBack(0.0); + queue.pushBack(1.0); + queue.pushBack(2.0); + queue.pushBack(3.0); + queue.popFront(); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.reset(); + + /* Buffer contains {_, _, 1, 2, 3} + * ^ front + */ + queue.pushBack(0.0); + queue.pushBack(0.0); + queue.pushBack(1.0); + queue.pushBack(2.0); + queue.pushBack(3.0); + queue.popFront(); + queue.popFront(); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.reset(); + + /* Buffer contains {3, _, _, 1, 2} + * ^ front + */ + queue.pushBack(3.0); + queue.pushFront(2.0); + queue.pushFront(1.0); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.reset(); + + /* Buffer contains {2, 3, _, _, 1} + * ^ front + */ + queue.pushBack(2.0); + queue.pushBack(3.0); + queue.pushFront(1.0); + + queue.resize(2); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + queue.resize(5); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + + // Test pushBack() after resize + queue.pushBack(3.0); + assertEquals(1.0, queue.get(0), 0.00005); + assertEquals(2.0, queue.get(1), 0.00005); + assertEquals(3.0, queue.get(2), 0.00005); + + // Test pushFront() after resize + queue.pushFront(4.0); + assertEquals(4.0, queue.get(0), 0.00005); + assertEquals(1.0, queue.get(1), 0.00005); + assertEquals(2.0, queue.get(2), 0.00005); + assertEquals(3.0, queue.get(3), 0.00005); + } }