void IKGoal::RemoveRotationAxis(const Vector3& p,const Vector3& axis)
{
switch(rotConstraint) {
case 0: //done
break;
case 1: //single-axis rotation
FatalError("TODO - set dual-axis rotation");
break;
case 2: //dual-axis rotation
//if one of the axes are parallel to the normal, all is well
FatalError("TODO - dual-axis rotation isn't implemented");
break;
case 3: //fixed rotation
//allow it to rotate about the cp normal
{
Vector3 locAxis;
Vector3 locPos;
RigidTransform T;
GetFixedGoalTransform(T);
T.R.mulTranspose(axis,locAxis);
T.mulInverse(p,locPos);
SetAxisRotation(locAxis,axis);
localPosition = locPos;
endPosition = p;
}
break;
}
}
void IKSolver::getResidual(std::vector<double>& out)
{
int size = 0;
for(size_t i=0;i<objectives.size();i++) {
int m=IKGoal::NumDims(objectives[i].goal.posConstraint);
int n=IKGoal::NumDims(objectives[i].goal.rotConstraint);
size += m + n;
}
out.resize(size);
size = 0;
for(size_t i=0;i<objectives.size();i++) {
const IKGoal& goal = objectives[i].goal;
int m=IKGoal::NumDims(goal.posConstraint);
int n=IKGoal::NumDims(goal.rotConstraint);
Real poserr[3],orierr[3];
if(goal.destLink < 0)
goal.GetError(robot.robot->links[goal.link].T_World,poserr,orierr);
else {
RigidTransform Trel;
Trel.mulInverseB(robot.robot->links[goal.link].T_World,robot.robot->links[goal.destLink].T_World);
goal.GetError(Trel,poserr,orierr);
}
for(int k=0;k<m;k++,size++)
out[size] = poserr[k];
for(int k=0;k<n;k++,size++)
out[size] = orierr[k];
}
}
void NearbyPoints(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol,std::vector<int>& pointIds,size_t maxContacts)
{
Box3D bb;
GetBB(pc,bb);
//quick reject test
if(g.Distance(bb) > tol) return;
GeometricPrimitive3D glocal = g;
RigidTransform Tinv;
Tinv.setInverse(pc.currentTransform);
glocal.Transform(Tinv);
AABB3D gbb = glocal.GetAABB();
gbb.setIntersection(pc.bblocal);
//octree overlap method
vector<Vector3> points;
vector<int> ids;
pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids);
for(size_t i=0;i<points.size();i++)
if(glocal.Distance(points[i]) <= tol) {
pointIds.push_back(ids[i]);
if(pointIds.size()>=maxContacts) return;
}
return;
/*
//grid enumeration method
GridSubdivision::Index imin,imax;
pc.grid.PointToIndex(Vector(3,gbb.bmin),imin);
pc.grid.PointToIndex(Vector(3,gbb.bmax),imax);
int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1);
if(numCells > (int)pc.points.size()) {
printf("Testing all points\n");
//test all points, linearly
for(size_t i=0;i<pc.points.size();i++)
if(glocal.Distance(pc.points[i]) <= tol) {
pointIds.push_back(int(i));
if(pointIds.size()>=maxContacts) return;
}
}
else {
printf("Testing points in BoxQuery\n");
gNearbyTestThreshold = tol;
gNearbyTestResults.resize(0);
gNearbyTestObject = &glocal;
gNearbyTestBranch = maxContacts;
pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),nearbyTest);
pointIds.resize(gNearbyTestResults.size());
for(size_t i=0;i<points.size();i++)
pointIds[i] = gNearbyTestResults[i] - &pc.points[0];
}
*/
}
/// \brief transform the center of mass and inertia matrix by trans
void OrXmlMass::ChangeCoordinateSystem(RigidTransform& trans)
{
// Vector oldcom = t.trans;
// TransformMatrix trot = matrixFromQuat(trans.rot);
// t = trot.rotate(t.rotate(trot.inverse())); // rotate inertia
// t.trans = trans*oldcom;
if(!com || !inertia){
cout<<"No com or inertia specified!"<<flush;
getchar();
}
Vector3 oldcom(*com);
RigidTransform trot, inverseT, massT, tmpT, finalmassT;
trot.R.set(trans.R);
trot.t.setZero();
massT.R.set(*inertia);
massT.t.set(*com);
trot.getInverse(inverseT);
tmpT.mul(massT, inverseT);
finalmassT.mul(trot, tmpT);
trans.mul(oldcom, finalmassT.t);
com->set(finalmassT.t);
inertia->set(finalmassT.R);
}
void WorkspaceBound::SetTransformed(const WorkspaceBound& w, RigidTransform& T)
{
T.mulPoint(w.center,center);
outerRadius = w.outerRadius;
innerRadius = w.innerRadius;
maxAngle = w.maxAngle;
}
void WorkspaceBound::SetTransformed(const WorkspaceBound& w, RigidTransform& T)
{
balls.resize(w.balls.size());
for(size_t i=0;i<w.balls.size();i++) {
T.mulPoint(w.balls[i].center,balls[i].center);
balls[i].outerRadius = w.balls[i].outerRadius;
balls[i].innerRadius = w.balls[i].innerRadius;
}
maxAngle = w.maxAngle;
}
bool WithinDistance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol)
{
Box3D bb;
GetBB(pc,bb);
//quick reject test
if(g.Distance(bb) > tol) return false;
GeometricPrimitive3D glocal = g;
RigidTransform Tinv;
Tinv.setInverse(pc.currentTransform);
glocal.Transform(Tinv);
AABB3D gbb = glocal.GetAABB();
gbb.setIntersection(pc.bblocal);
//octree overlap method
vector<Vector3> points;
vector<int> ids;
pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids);
for(size_t i=0;i<points.size();i++)
if(glocal.Distance(points[i]) <= tol) return true;
return false;
/*
//grid enumeration method
GridSubdivision::Index imin,imax;
pc.grid.PointToIndex(Vector(3,gbb.bmin),imin);
pc.grid.PointToIndex(Vector(3,gbb.bmax),imax);
int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1);
if(numCells > (int)pc.points.size()) {
//test all points, linearly
for(size_t i=0;i<pc.points.size();i++)
if(glocal.Distance(pc.points[i]) <= tol) return true;
return false;
}
else {
gWithinDistanceTestThreshold = tol;
gWithinDistanceTestObject = &glocal;
bool collisionFree = pc.grid.IndexQuery(imin,imax,withinDistanceTest);
return !collisionFree;
}
*/
}
Real Distance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g)
{
GeometricPrimitive3D glocal = g;
RigidTransform Tinv;
Tinv.setInverse(pc.currentTransform);
glocal.Transform(Tinv);
/*
AABB3D gbb = glocal.GetAABB();
gDistanceTestValue = Inf;
gDistanceTestObject = &glocal;
pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),distanceTest);
return gDistanceTestValue;
*/
//test all points, linearly
Real dmax = Inf;
for(size_t i=0;i<pc.points.size();i++)
dmax = Min(dmax,glocal.Distance(pc.points[i]));
return dmax;
}
Real RobotIKError(const RobotKinematics3D& robot,const IKGoal& goal)
{
int m=IKGoal::NumDims(goal.posConstraint);
int n=IKGoal::NumDims(goal.rotConstraint);
Real poserr[3],orierr[3];
if(goal.destLink < 0)
goal.GetError(robot.links[goal.link].T_World,poserr,orierr);
else {
RigidTransform Trel;
Trel.mulInverseB(robot.links[goal.link].T_World,robot.links[goal.destLink].T_World);
goal.GetError(Trel,poserr,orierr);
}
Real emax=0.0;
for(int i=0;i<m;i++)
emax = Max(emax,Abs(poserr[i]));
for(int i=0;i<n;i++)
emax = Max(emax,Abs(orierr[i]));
return emax;
}
void OrientedSupportPolygon::GetXYSlice(Real height,UnboundedPolytope2D& poly) const
{
//transform SP space to original space
RigidTransform Tinv;
Tinv.setInverse(T);
Vector3 x,y,z;
Tinv.R.get(x,y,z);
Matrix2 R2;
R2(0,0) = T.R(0,0);
R2(0,1) = T.R(0,1);
R2(1,0) = T.R(1,0);
R2(1,1) = T.R(1,1);
Real det=R2.determinant();
Assert(Abs(det) > Epsilon); //otherwise the slice is infinite...
Assert(Abs(z.z) > Epsilon); //otherwise the slice is infinite...
bool flip = (det < 0.0); //the plane is flipped
poly.vertices.resize(sp.vertices.size());
for(size_t i=0;i<sp.vertices.size();i++) {
const PointRay2D& v=sp.vertices[i];
Real u;
//let p = [v.x,v.y,u] (u unknown), q be the transformed point
if(v.isRay) {
//pick u s.t. p = T*[q.x,q.y,0] //parallel to xy plane
u = (-x.z*v.x - y.z*v.y)/z.z;
poly.vertices[i].isRay = true;
poly.vertices[i].x = x.x*v.x + y.x*v.y + u*z.x;
poly.vertices[i].y = x.y*v.x + y.y*v.y + u*z.y;
}
else {
//pick u s.t. p = T*[q.x,q.y,height]
u = (height - x.z*v.x - y.z*v.y - Tinv.t.z)/z.z;
poly.vertices[i].isRay = false;
poly.vertices[i].x = x.x*v.x + y.x*v.y + u*z.x + Tinv.t.x;
poly.vertices[i].y = x.y*v.x + y.y*v.y + u*z.y + Tinv.t.y;
}
}
if(flip) //maintain CCW ordering
reverse(poly.vertices.begin(),poly.vertices.end());
//laziness
poly.CalcPlanes();
}
/** Two particles
*/
void test_2particles( Real stiffness, Real dampingRatio, Real restLength,
Vec3 x0, Vec3 v0,
Vec3 x1, Vec3 v1,
Vec3 f0)
{
// set position and velocity vectors, using DataTypes::set to cope with tests in dimension 2
VecCoord x(2);
DataTypes::set( x[0], x0[0],x0[1],x0[2]);
DataTypes::set( x[1], x1[0],x1[1],x1[2]);
VecDeriv v(2);
DataTypes::set( v[0], v0[0],v0[1],v0[2]);
DataTypes::set( v[1], v1[0],v1[1],v1[2]);
VecDeriv f(2);
DataTypes::set( f[0], f0[0], f0[1], f0[2]);
DataTypes::set( f[1], -f0[0],-f0[1],-f0[2]);
// randomly rotate and translate the scene
/// @todo functions to apply the transform to VecCoord and VecDeriv
RigidTransform<DataTypes> transform;
transform.randomize(1.1);
transform.projectPoint( x[0]);
transform.projectVector(v[0]);
transform.projectPoint( x[1]);
transform.projectVector(v[1]);
transform.projectVector(f[0]);
transform.projectVector(f[1]);
// tune the force field
this->force->addSpring(0,1,stiffness,dampingRatio,restLength);
// cout<<"test_2particles, x = " << x << endl;
// cout<<" v = " << v << endl;
// cout<<" f = " << f << endl;
// and run the test
this->run_test( x, v, f );
}
