Commits

bergsoe committed d5b1c7d

Use std::copy.

Comments (0)

Files changed (1)

c_robwork/papl_robwork.cpp

 #include <rw/proximity/CollisionDetector.hpp>
 #include <rwlibs/proximitystrategies/ProximityStrategyYaobi.hpp>
 #include <sstream>
+#include <algorithm>
 
 using namespace rw::models;
 using namespace rw::math;
 
 const char* robwork_getWorkCellError(const RW_WorkCell* workcell)
 {
+    RW_ASSERT(workcell);
+
     return workcell->error.c_str();
 }
 
 RW_State* robwork_getState(const RW_WorkCell* workcell)
 {
     RW_ASSERT(workcell);
+
     return new RW_State(workcell->workcell->getDefaultState());
 }
 
 RW_Device* robwork_getDevice(const RW_WorkCell* workcell, const char* name)
 {
+    RW_ASSERT(workcell);
+
     return new RW_Device(workcell->workcell->findDevice(name));
 }
 
 int robwork_getDOF(const RW_Device* device)
 {
     RW_ASSERT(device);
+
     return (int)device->device->getDOF();
 }
 
 {
     void writeQ(double* vals, const Q& q)
     {
-        const int len = (int)q.size();
-        for (int i = 0; i < len; ++i) {
-            vals[i] = q[i];
-        }
+        std::copy(q.begin(), q.begin() + q.size(), vals);
     }
 }
 
 
 RW_State* robwork_copyState(const RW_State* state)
 {
+    RW_ASSERT(state);
+
     return new RW_State(state->state);
 }
 
 void robwork_addTimedState(
     RW_TimedStatePath* path, double time, const RW_State* state)
 {
+    RW_ASSERT(path);
+    RW_ASSERT(state);
+
     path->path.push_back(Timed<State>(time, state->state));
 }
 
     const RW_TimedStatePath* path,
     const char* file)
 {
+    RW_ASSERT(workcell);
+    RW_ASSERT(path);
+    RW_ASSERT(file);
+
     try {
         PathLoader::storeTimedStatePath(
             *workcell->workcell,