void testInsert() {
const int data1[3] = {7, -2, 3};
const int data2[4] = {101, 121, -111, 321};
int wdata[3] = {99, 9999, 999}; // writable
Array_<int> a(data2,data2+4); // copy the data
SimTK_TEST(&a[0] != data2);
ArrayViewConst_<int> avc(data2, data2+4); // share the data
SimTK_TEST(&avc[1] == data2+1);
Array_<int> aw(wdata, wdata+3, DontCopy()); // shared
SimTK_TEST(&aw[0] == wdata);
// Can't insert into non-owner.
SimTK_TEST_MUST_THROW(aw.insert(aw.begin(), avc.begin(), avc.end()));
// Unless we're inserting zero elements; that's allowed.
aw.insert(&aw[1], avc.begin(), avc.begin());
Array_<int> ac(data1, data1+3);
std::vector<int> vc(data1, data1+3);
ac.insert(&ac[1], &a[1], &a[1]+2);
vc.insert(vc.begin()+1, &a[1], &a[1]+2);
SimTK_TEST(ac.size()==5);
SimTK_TEST(ac == vc); // 7, 121, -111, -2, 3
// insert vc onto beginning and end of ac
ac.insert(ac.begin(), vc.begin(), vc.end());
ac.insert(ac.end(), vc.begin(), vc.end());
SimTK_TEST(ac.size()==15);
SimTK_TEST(ac(0,5)==vc && ac(5,5)==vc && ac(10,5)==vc);
// Shrink ac back down to 5 again.
ac.erase(ac.begin()+2, ac.begin()+12);
SimTK_TEST(ac == vc);
SimTK_TEST(ac.allocated() >= 15);
ac.shrink_to_fit();
SimTK_TEST(ac.allocated() < 15);
SimTK_TEST(ac == vc); // make sure we didn't lose the data
// Now try some null insertions
Array_<int> null;
ac.insert(ac.begin(), null.begin(), null.end());
ac.insert(ac.begin()+2, null.begin(), null.end());
ac.insert(ac.end(), null.begin(), null.end());
ac.insert(ac.begin(), 0, 929);
ac.insert(ac.begin()+2, 0, 929);
ac.insert(ac.end(), 0, 929);
SimTK_TEST(ac == vc);
ArrayView_<int> null2;
null.insert(null.begin(), null2.begin(), null2.end());
SimTK_TEST(null.empty() && null2.empty());
// How about inserting into a null array?
null.insert(null.begin(), 3, 987);
SimTK_TEST(null == std::vector<int>(3,987));
null.deallocate(); // back to null
SimTK_TEST(null.data()==0 && null.size()==0 && null.allocated()==0);
null.insert(null.begin(), ac.begin(), ac.end());
SimTK_TEST(null == vc);
null.deallocate();
// Fill in a bunch of 1000's in the middle, erase the beginning and
// end, and make sure we see just the 1000's.
ac.insert(ac.begin()+2, 99, 1000);
ac.erase(ac.begin(), ac.begin()+2);
ac.erase(ac.end()-3, ac.end());
SimTK_TEST(ac == Array_<int>(99,1000));
}
void ContactGeometry::TriangleMesh::Impl::createObbTree
(OBBTreeNodeImpl& node, const Array_<int>& faceIndices)
{ // Find all vertices in the node and build the OrientedBoundingBox.
node.numTriangles = faceIndices.size();
set<int> vertexIndices;
for (int i = 0; i < (int) faceIndices.size(); i++)
for (int j = 0; j < 3; j++)
vertexIndices.insert(faces[faceIndices[i]].vertices[j]);
Vector_<Vec3> points((int)vertexIndices.size());
int index = 0;
for (set<int>::iterator iter = vertexIndices.begin();
iter != vertexIndices.end(); ++iter)
points[index++] = vertices[*iter].pos;
node.bounds = OrientedBoundingBox(points);
if (faceIndices.size() > 3) {
// Order the axes by size.
int axisOrder[3];
const Vec3& size = node.bounds.getSize();
if (size[0] > size[1]) {
if (size[0] > size[2]) {
axisOrder[0] = 0;
if (size[1] > size[2]) {
axisOrder[1] = 1;
axisOrder[2] = 2;
}
else {
axisOrder[1] = 2;
axisOrder[2] = 1;
}
}
else {
axisOrder[0] = 2;
axisOrder[1] = 0;
axisOrder[2] = 1;
}
}
else if (size[0] > size[2]) {
axisOrder[0] = 1;
axisOrder[1] = 0;
axisOrder[2] = 2;
}
else {
if (size[1] > size[2]) {
axisOrder[0] = 1;
axisOrder[1] = 2;
}
else {
axisOrder[0] = 2;
axisOrder[1] = 1;
}
axisOrder[2] = 0;
}
// Try splitting along each axis.
for (int i = 0; i < 3; i++) {
Array_<int> child1Indices, child2Indices;
splitObbAxis(faceIndices, child1Indices, child2Indices,
axisOrder[i]);
if (child1Indices.size() > 0 && child2Indices.size() > 0) {
// It was successfully split, so create the child nodes.
node.child1 = new OBBTreeNodeImpl();
node.child2 = new OBBTreeNodeImpl();
createObbTree(*node.child1, child1Indices);
createObbTree(*node.child2, child2Indices);
return;
}
}
}
// This is a leaf node.
node.triangles.insert(node.triangles.begin(), faceIndices.begin(),
faceIndices.end());
}
