/************************Rotation Matrix****************************
* Give the Rotation Axis and the Rotation Angle
* Generate the Rotation Matrix
* Author: C.Y.
* Date 2006.11.30.
*************************End***************************************/
inline bool RotationMatrix(const myvector &axis, double angle, matrix &romtx)
{
if(axis.size()!=3)
{
cout<<"Error 1 in RotationMatrix()"<<endl;
return false;
}
myvector ouc(3, 0);
if(!Norm2Vector(axis, ouc))
{
cout<<"Error 2 in RotationMatrix()"<<endl;
return false;
}
int i=0, j=0;
matrix su, u_i, unit, tmp;
SetMatrix(su, 3, 3);
su[0][0]=0; su[0][1]=-ouc[2]; su[0][2]=ouc[1];
su[1][0]=ouc[2]; su[1][1]=0; su[1][2]=-ouc[0];
su[2][0]=-ouc[1]; su[2][1]=ouc[0]; su[2][2]=0;
RealTimesMatrix(sin(angle), su, su);
TransVectorTimesVector(ouc, ouc, u_i);
SetMatrix(unit, 3, 3);
SetMatrix(tmp, 3, 3);
unit[0][0]=1; unit[1][1]=1; unit[2][2]=1;
RealTimesMatrix(-1, u_i, tmp);
MatrixAddMatrix(unit, tmp, tmp);
RealTimesMatrix(cos(angle), tmp, tmp);
MatrixAddMatrix(u_i, tmp, tmp);
MatrixAddMatrix(su, tmp, tmp);
SetMatrix(romtx, 4, 4);
for(i=0; i<tmp.size(); ++i)
for(j=0; j<tmp[i].size(); ++j)
romtx[i][j]=tmp[i][j];
romtx[0][3]=0; romtx[1][3]=0; romtx[2][3]=0;
romtx[3][0]=0; romtx[3][1]=0; romtx[3][2]=0;
romtx[3][3]=1;
return true;
}
//Outer vector: list
//Inner vector: triples of nodeids
void add_facts(vector<vector<nodeid>> facts)
{
//std::map<nodeid,std::vector<std::pair<Thing*,Thing*>>>
ths_t &ths = *new ths_t;
ths_garbage = &ths;///.push_back(ths);
//Map each pred to a list of it's subject/object pairs
for (auto f: facts)
ths[f[1]].push_back({
create_node(f[0]),
create_node(f[2])});
coro suc, ouc;
//For each pred in the ths:
//std::pair<nodeid,std::vector<std::pair<Thing*,Thing*>>>
for (auto ff:ths)
{
//std::vector<std::pair<Thing*, Thing*>>
auto &pairs = ff.second;
EEE; //char entry = 0;
pos_t pi = 0;
//Map each pred to a coro on it's subject/object
builtins[ff.first].push_back([suc,ouc,pi,pairs,entry](Thing *s_, Thing *o_) mutable{
switch(entry)
{
case 0:
//hrmm.. where is pi being incremented? looks
//like this just does pi=0 then exits.
/*yea looks like you found a bug
this thing looks pretty suboptimal btw...buti guess it should get the job done*/
if (pi < pairs.size())//fixed?
{
//Generate a unify coro for the subject;
//on the fly.
//Then run it.
suc = unify(s_,&pairs[pi].first);
while(suc()){
//Again for the object
ouc = unify(o_,&pairs[pi].second);
while(ouc())
{
//If both succeed then yield
//true.
//not quite
entry = LAST;
return true;
/*
entry = LAST;
so...wanna fix this?
*/
case_LAST:;
}
}
}
END;
}
});
}
}
