void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
{
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
updateWheelTransformsWS(wheel,interpolatedTransform);
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
btVector3 fwd = up.cross(right);
fwd = fwd.normalize();
// up = right.cross(fwd);
// up.normalize();
//rotate around steering over de wheelAxleWS
btScalar steering = wheel.m_steering;
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
btMatrix3x3 steeringMat(steeringOrn);
btQuaternion rotatingOrn(right,-wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn);
btMatrix3x3 basis2(
right[0],fwd[0],up[0],
right[1],fwd[1],up[1],
right[2],fwd[2],up[2]
);
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
wheel.m_worldTransform.setOrigin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
);
}
void VehicleBody::_update_wheel(int p_idx, PhysicsDirectBodyState *s) {
VehicleWheel &wheel = *wheels[p_idx];
_update_wheel_transform(wheel, s);
Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS;
Vector3 fwd = up.cross(right);
fwd = fwd.normalized();
//rotate around steering over de wheelAxleWS
real_t steering = wheel.steers ? m_steeringValue : 0.0;
Basis steeringMat(up, steering);
Basis rotatingMat(right, wheel.m_rotation);
Basis basis2(
right[0], up[0], fwd[0],
right[1], up[1], fwd[1],
right[2], up[2], fwd[2]);
wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2);
//wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat));
wheel.m_worldTransform.set_origin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
}
int main()
{
//set up basisvectors
std::vector<std::vector<double> > basis2(2,std::vector<double>(2));
basis2[0][0] = 1;
basis2[0][1] = 0;
basis2[1][0] = 0;
basis2[1][1] = 1;
//define lattice
Lattice myLattice(2,basis2);
//set up 2d polytopes
std::vector<std::vector<double> > ver0(3,std::vector<double>(2));
std::vector<std::vector<double> > ver1(4,std::vector<double>(2));
std::vector<std::vector<double> > ver2(4,std::vector<double>(2));
std::vector<std::vector<double> > ver3(3,std::vector<double>(2));
std::vector<std::vector<double> > ver4(5,std::vector<double>(2));
std::vector<std::vector<double> > ver5(4,std::vector<double>(2));
std::vector<std::vector<double> > ver6(6,std::vector<double>(2));
std::vector<std::vector<double> > ver7(4,std::vector<double>(2));
std::vector<std::vector<double> > ver8(5,std::vector<double>(2));
std::vector<std::vector<double> > ver9(3,std::vector<double>(2));
std::vector<std::vector<double> > ver10(4,std::vector<double>(2));
ver0[0][0] = 1;
ver0[0][1] = 0;
ver0[1][0] = 0;
ver0[1][1] = 1;
ver0[2][0] = -1;
ver0[2][1] = -1;
ver1[0][0] = 1;
ver1[0][1] = 0;
ver1[1][0] = 0;
ver1[1][1] = 1;
ver1[2][0] = -1;
ver1[2][1] = 0;
ver1[3][0] = 0;
ver1[3][1] = -1;
ver2[0][0] = 1;
ver2[0][1] = 0;
ver2[1][0] = 0;
ver2[1][1] = 1;
ver2[2][0] = -1;
ver2[2][1] = 0;
ver2[3][0] = -1;
ver2[3][1] = -1;
ver3[0][0] = 1;
ver3[0][1] = 0;
ver3[1][0] = 0;
ver3[1][1] = 1;
ver3[2][0] = -2;
ver3[2][1] = -1;
ver4[0][0] = 1;
ver4[0][1] = 0;
ver4[1][0] = 1;
ver4[1][1] = 1;
ver4[2][0] = 0;
ver4[2][1] = 1;
ver4[3][0] = -1;
ver4[3][1] = 0;
ver4[4][0] = 0;
ver4[4][1] = -1;
ver5[0][0] = 1;
ver5[0][1] = 0;
ver5[1][0] = 0;
ver5[1][1] = 1;
ver5[2][0] = -1;
ver5[2][1] = 1;
ver5[3][0] = -1;
ver5[3][1] = -1;
Polytope pol0(ver0,myLattice);
Polytope pol1(ver1,myLattice);
Polytope pol2(ver2,myLattice);
Polytope pol3(ver3,myLattice);
Polytope pol4(ver4,myLattice);
Polytope pol5(ver5,myLattice);
Polytope pol15 = pol0.getCorrespondingDualPolytope();
Polytope pol14 = pol1.getCorrespondingDualPolytope();
Polytope pol13 = pol2.getCorrespondingDualPolytope();
Polytope pol12 = pol3.getCorrespondingDualPolytope();
Polytope pol11 = pol4.getCorrespondingDualPolytope();
Polytope pol10 = pol5.getCorrespondingDualPolytope();
ver6[0][0] = 1;
ver6[0][1] = 0;
ver6[1][0] = 1;
ver6[1][1] = 1;
ver6[2][0] = 0;
ver6[2][1] = 1;
ver6[3][0] = -1;
ver6[3][1] = 0;
ver6[4][0] = -1;
ver6[4][1] = -1;
ver6[5][0] = 0;
ver6[5][1] = -1;
ver7[0][0] = 1;
ver7[0][1] = 0;
ver7[1][0] = -1;
ver7[1][1] = 1;
ver7[2][0] = -1;
ver7[2][1] = -1;
ver7[3][0] = 1;
ver7[3][1] = -1;
ver8[0][0] = 1;
ver8[0][1] = 0;
ver8[1][0] = 1;
ver8[1][1] = 1;
ver8[2][0] = -1;
ver8[2][1] = 1;
ver8[3][0] = -1;
ver8[3][1] = 0;
ver8[4][0] = 0;
ver8[4][1] = -1;
ver9[0][0] = 1;
ver9[0][1] = 0;
ver9[1][0] = -1;
ver9[1][1] = 2;
ver9[2][0] = -1;
ver9[2][1] = -1;
Polytope pol6(ver6,myLattice);
Polytope pol7(ver7,myLattice);
Polytope pol8(ver8,myLattice);
Polytope pol9(ver9,myLattice);
//Polytope polytopes2D[16] = {pol0, pol1, pol2, pol3, pol4, pol5, pol6, pol7, pol8, pol9, pol10, pol11, pol12, pol13, pol14, pol15};
//Polytope polytopes2DDual[16] = {pol15, pol14, pol13, pol12, pol11, pol10, pol6, pol7, pol8, pol9, pol5, pol4, pol3, pol2, pol1, pol0};
std::vector<Polytope > myList = {pol0, pol1, pol2, pol3, pol4, pol5, pol6, pol7, pol8, pol9, pol10, pol11, pol12, pol13, pol14, pol15};
std::vector<Polytope > myFiveVerticesList = {pol0, pol3, pol9, pol12, pol15};
//Construct 4d polytopes from this data
std::string fileName = "output";
//pol0.printListFrom2DTo4DPolytopesToFile(fileName, myList);
pol0.printListFrom2DTo4DPolytopesToFile("outputForFiveVertices", myFiveVerticesList);
}
Matrix<double> BspCurvBasisFuncSet::CreateMatrixIntegral(int lev1, int lev2) const
{
KnotSet kset1 = KnotSet(*kts,ord,num).CreateKnotSetDeriv(lev1);
KnotSet kset2 = KnotSet(*kts,ord,num).CreateKnotSetDeriv(lev2);
Matrix<double> mat(kset1.GetNum()-(ord-lev1),kset2.GetNum()-(ord-lev2));
BspCurvBasisFuncSet basis1(kset1.GetKnots(),ord-lev1,kset1.GetNum());
BspCurvBasisFuncSet basis2(kset2.GetKnots(),ord-lev2,kset2.GetNum());
// find the first basis function for which the last distinct
// knot is greater than x1
Matrix<double> mat1(kset1.GetNum()-ord+lev1,kset2.GetNum()-ord+lev2,0.0);
for (int i=0; i<kset1.GetNum()-ord+lev1; i++)
for (int j=0; j<kset2.GetNum()-ord+lev2; j++) {
// create the two std::sets representing the two knot std::sets
Vector<double> temp1((*(basis1.b))[i].GetKnots());
Vector<double> temp2((*(basis2.b))[j].GetKnots());
std::set<double> s1(temp1.begin(),temp1.end());
std::set<double> s2(temp2.begin(),temp2.end());
if (*(--s2.end()) > *(s1.begin())) {
// form the intersection
std::set<double> s3;
std::set_intersection(s1.begin(),s1.end(),s2.begin(),s2.end(),std::inserter(s3,s3.begin()));
// if there is an intersection
if (s3.size() > 1) {
Vector<double> v(s3.size());
std::set<double>::iterator s = s3.begin();
// copy the elements into a vector
for (unsigned int k=0; k<s3.size(); k++) v[k] = *s++;
// create the compbezcurvs
Vector<BezCurv<double> > vec1(s3.size()-1);
Vector<BezCurv<double> > vec2(s3.size()-1);
BspCurv<double> b1((*(basis1.b))[i].GetBspCurv()), b2((*(basis2.b))[j].GetBspCurv());
// find the segments of intersection
for (unsigned int k=0; k<s3.size()-1; k++) {
int segb1 = b1.GetKnotSet().Find_segment(v[k]);
int segb2 = b2.GetKnotSet().Find_segment(v[k]);
vec1[k] = b1.GetSegment(segb1);
vec2[k] = b2.GetSegment(segb2);
}
CompBezCurv<double> cb1(vec1,s3.size()-1,v);
CompBezCurv<double> cb2(vec2,s3.size()-1,v);
CompBezCurv<double> prod = cb1.Product(cb2);
mat1[i][j] = prod.ConvertBspCurv().Integrate((*kts)[ord-1],(*kts)[num-ord]);
}
}
}
return mat1;
}
