void CTableFrameSink::logCard( WORD wChairID )
{
IServerUserItem *pIServerUserItem=m_pITableFrame->GetServerUserItem(wChairID);
if(pIServerUserItem == NULL) return;
m_oLog.Log(TEXT("玩家：%s 机器人[%s] 手牌：[%s%s][%s%s][%s%s][%s%s][%s%s] 牌型：%s") , pIServerUserItem->GetAccounts() , pIServerUserItem->IsAndroidUser()?"是":"否" ,
cn(m_cbHandCardData[wChairID][0]),vn(m_cbHandCardData[wChairID][0]),cn(m_cbHandCardData[wChairID][1]),vn(m_cbHandCardData[wChairID][1]),cn(m_cbHandCardData[wChairID][2]),vn(m_cbHandCardData[wChairID][2]),
cn(m_cbHandCardData[wChairID][3]),vn(m_cbHandCardData[wChairID][3]),cn(m_cbHandCardData[wChairID][4]),vn(m_cbHandCardData[wChairID][4]) , CardTypeName(m_GameLogic.GetBestCardType(m_cbHandCardData[wChairID],MAX_COUNT)));
m_oLog.Log(TEXT("玩家：%s 机器人[%s] 奖励牌：[%s%s][%s%s][%s%s][%s%s][%s%s] 相同牌数：%d" ) , pIServerUserItem->GetAccounts() , pIServerUserItem->IsAndroidUser()?"是":"否" ,
cn(m_cbRewardCardData[wChairID][0]),vn(m_cbRewardCardData[wChairID][0]),cn(m_cbRewardCardData[wChairID][1]),vn(m_cbRewardCardData[wChairID][1]),cn(m_cbRewardCardData[wChairID][2]),vn(m_cbRewardCardData[wChairID][2]),
cn(m_cbRewardCardData[wChairID][3]),vn(m_cbRewardCardData[wChairID][3]),cn(m_cbRewardCardData[wChairID][4]),vn(m_cbRewardCardData[wChairID][4]),m_GameLogic.GetSameCount(m_cbHandCardData[wChairID], m_cbRewardCardData[wChairID]) );
}
static void make_quad( float x0, float y0, float z0, float x1, float y1, float z1, float height, QVector<float> &data, bool addNormals )
{
float dx = x1 - x0;
float dy = -( y1 - y0 );
// perpendicular vector in plane to [x,y] is [-y,x]
QVector3D vn( -dy, 0, dx );
vn.normalize();
// triangle 1
data << x0 << z0 + height << -y0;
if ( addNormals )
data << vn.x() << vn.y() << vn.z();
data << x1 << z1 + height << -y1;
if ( addNormals )
data << vn.x() << vn.y() << vn.z();
data << x0 << z0 << -y0;
if ( addNormals )
data << vn.x() << vn.y() << vn.z();
// triangle 2
data << x0 << z0 << -y0;
if ( addNormals )
data << vn.x() << vn.y() << vn.z();
data << x1 << z1 + height << -y1;
if ( addNormals )
data << vn.x() << vn.y() << vn.z();
data << x1 << z1 << -y1;
if ( addNormals )
data << vn.x() << vn.y() << vn.z();
}
void graph3d::grid_draw(void)
{
mesh_material::set_default();
VECT vn(0.0f,0.0f,-1.0f);
VECT vf(0.0f,0.0f, 1.0f);
GPIPE *p_gpipe = gpipe_get();
p_gpipe->screen_normal_to_world(&vn);
p_gpipe->screen_normal_to_world(&vf);
// v1->v2
VECT dir;
dir = vf-vn;
dir *= 0.5f;
vn += dir;
vn.y = 0.0f;
VECT vl(-1.0f,0.0f,1.0f);
VECT vr( 1.0f,0.0f,1.0f);
p_gpipe->screen_normal_to_world(&vl);
p_gpipe->screen_normal_to_world(&vr);
VECT width;
width = vr-vl;
draw_floor_line(&vn,width.size(),2,40);
}
XYZ Z3DTexture::testRaster(const XYZ& ori)
{
if(!m_pTree) return XYZ(0);
raster->clear();
m_pTree->setSampleOpacity(0.05f);
m_pTree->setRaster(raster);
m_pTree->occlusionAccum(ori);
raster->draw();
XYZ coe[SH_N_BASES];
m_sh->zeroCoeff(coe);
float l, vl;
XYZ vn(0,1,0);
for(unsigned int j=0; j<SH_N_SAMPLES; j++) {
SHSample s = m_sh->getSample(j);
vl = vn.dot(s.vector);
if(vl>0) {
raster->readLight(s.vector, l);
l *= vl;
m_sh->projectASample(coe, j, l);
}
}
m_sh->reconstructAndDraw(coe);
return m_sh->integrate(m_sh->constantCoeff, coe);
}
void CMesh::generateFlatNormals()
{
if (hasNormals())
return;
// for each face...
for (vector<SPolygonFace>::iterator itFace = m_faceVector.begin(); itFace != m_faceVector.end(); ++itFace)
{
const SVertex& v1 = m_vertVector[(*itFace).v[0]];
const SVertex& v2 = m_vertVector[(*itFace).v[1]];
const SVertex& v3 = m_vertVector[(*itFace).v[2]];
SVertex v(v2.x - v1.x, v2.y - v1.y, v2.z - v1.z);
SVertex w(v3.x - v1.x, v3.y - v1.y, v3.z - v1.z);
SVertex vn(v.y*w.z - v.z*w.y, v.z*w.x - v.x*w.z, v.x*w.y - v.y*w.x);
float length = sqrtf(vn.x*vn.x+vn.y*vn.y+vn.z*vn.z);
if (0 < length) {
const float a = 1/length;
vn.x *= a;
vn.y *= a;
vn.z *= a;
}
m_normVector.push_back(vn);
// register the generated normal through the face
fill((*itFace).vn.begin(), (*itFace).vn.end(), m_normVector.size()-1);
}
}
set<string> Attributes::namesOfWritableDoubleAttributes() const
{
const bool excludereadonly = true;
NamesOfDoubleAttributesVisitor vn(excludereadonly);
this->accept(vn);
set<string> rv = vn.names();
return rv;
}
std::vector<double> HoleModel::getNormals() {
double vnd[] = { 0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
1.0, 0.0, 0.0,
-1.0, 0.0, 0.0,
0.0, 0.0, -1.0
};
std::vector<double> vn(vnd, vnd + sizeof(vnd) / sizeof(double));
return vn;
}
static int vector_normalize(lua_State* l)
{
CelxLua celx(l);
celx.checkArgs(1, 1, "No arguments expected for vector:normalize");
Vec3d* v = this_vector(l);
Vec3d vn(*v);
vn.normalize();
celx.newVector(vn);
return 1;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "velodyne_post");
ros::NodeHandle nh("~");
try {
velodyne::VelodynePostNode vn(nh);
vn.spin();
}
catch (const std::exception& e) {
ROS_ERROR_STREAM("Exception: " << e.what());
return 1;
}
catch (...) {
ROS_ERROR_STREAM("Unknown Exception");
return 1;
}
return 0;
}
TEST_F(VecTest, ArithmeticFunc) {
vec3 east(1, 0, 0);
vec3 north(0, 1, 0);
vec3 up( cross(east, north) );
EXPECT_EQ(up, vec3(0,0,1));
EXPECT_EQ(dot(east, north), 0);
EXPECT_EQ(length(east), 1);
EXPECT_EQ(distance(east, north), sqrtf(2));
vec3 v0(1,2,3);
vec3 vn(normalize(v0));
EXPECT_FLOAT_EQ(1, length(vn));
EXPECT_FLOAT_EQ(length(v0), dot(v0, vn));
tvec3<double> vd(east);
EXPECT_EQ(length(vd), 1);
}
void Z3DAxis::setupCamera()
{
Z3DCamera camera;
glm::vec3 center(0.f);
camera.setFieldOfView(10.f);
float radius = 300.f;
float distance = radius/std::sin(glm::radians(camera.getFieldOfView())*0.5);
glm::vec3 vn(0, 0, 1); //plane normal
glm::vec3 position = center + vn * distance;
camera.setCamera(position, center, glm::vec3(0.0, 1.0, 0.0));
camera.setNearDist(distance - radius - 1);
camera.setFarDist(distance + radius);
m_rendererBase->setViewMatrix(camera.getViewMatrix(CenterEye));
m_rendererBase->setProjectionMatrix(camera.getProjectionMatrix(CenterEye));
}
TEUCHOS_UNIT_TEST(MultiVectorTransferFactory, Build)
{
out << "version: " << MueLu::Version() << std::endl;
out << "Tests the action of the transfer factory on a vector. In this test, the transfer is the tentative" << std::endl;
out << "prolongator, and the vector is all ones. So the norm of the resulting coarse grid vector should be" << std::endl;
out << "equal to the number of fine degrees of freedom." << std::endl;
Level fineLevel, coarseLevel;
TestHelpers::TestFactory<SC, LO, GO, NO, LMO>::createTwoLevelHierarchy(fineLevel, coarseLevel);
GO nx = 199;
RCP<Matrix> A = TestHelpers::TestFactory<SC, LO, GO, NO, LMO>::Build1DPoisson(nx);
fineLevel.Set("A",A);
RCP<MultiVector> fineOnes = MultiVectorFactory::Build(A->getRowMap(),1);
fineOnes->putScalar(1.0);
fineLevel.Set("onesVector",fineOnes);
RCP<TentativePFactory> TentativePFact = rcp(new TentativePFactory());
RCP<TransPFactory> RFact = rcp(new TransPFactory());
RCP<FactoryManager> M = rcp(new FactoryManager());
M->SetFactory("P", TentativePFact);
M->SetFactory("Ptent", TentativePFact);
M->SetFactory("R", RFact);
// fineLevel.SetFactoryManager(M);
coarseLevel.SetFactoryManager(M);
RCP<MueLu::MultiVectorTransferFactory<SC, LO, GO, NO, LMO> > mvtf = rcp(new MueLu::MultiVectorTransferFactory<SC, LO, GO, NO, LMO>("onesVector"));
mvtf->SetFactory("R",RFact);
coarseLevel.Request("onesVector",mvtf.get());
coarseLevel.Request("R",RFact.get());
coarseLevel.Request("P",TentativePFact.get());
mvtf->Build(fineLevel,coarseLevel);
RCP<MultiVector> coarseOnes = coarseLevel.Get<RCP<MultiVector> >("onesVector",mvtf.get());
Teuchos::Array<Teuchos::ScalarTraits<SC>::magnitudeType> vn(1);
coarseOnes->norm2(vn);
TEST_FLOATING_EQUALITY(vn[0]*vn[0],((SC)fineOnes->getGlobalLength()),1e-12);
} // Build test
void SceneObject::SetRotation( glm::vec3 & _axis, float _angle)
{
float sinAngle;
_angle *= 0.5f;
glm::vec3 vn(_axis);
glm::normalize(vn);
sinAngle = sin(_angle);
this->m_qRotation.x = (vn.x * sinAngle);
this->m_qRotation.y = (vn.y * sinAngle);
this->m_qRotation.z = (vn.z * sinAngle);
this->m_qRotation.w = cos(_angle);
m_matrix = glm::translate(
glm::mat4(1.0f),this->m_vOffset ) *
glm::mat4_cast( this->m_qRotation ) *
glm::scale( glm::mat4(1.0),glm::vec3(m_fScale,m_fScale,m_fScale)
);
this->m_localAABB = this->m_pModel->m_localAABB;
this->m_localAABB = this->m_pModel->m_localAABB;
this->m_localAABB.Transform(m_matrix);
}
void Cylinder::getMappedCoords(const Point& intersectPoint,
double& x, double &y) const
{
Point intersect = intersectPoint;
intersect.rotate(_rotation, true);
Point newPoint = intersect - _absolutePosition;
Vector vn(0, 0, -1);
Vector ve(1, 0, 0);
y = -newPoint._z;
newPoint.rotate(_rotation);
newPoint.normalize();
double phi = acos(- (vn * newPoint));
double theta = (acos((newPoint * ve) / sin(phi))) / (2 * M_PI);
vn *= ve;
double y2;
if (vn * newPoint > 0)
y2 = theta;
else
y2 = 1 - theta;
x = y2;
}
int KMeansClusterizer::reviseCenter(void) {
// pre-4. Store Initial Cluster ID
vi pre_id_v(getTrainDataSize());
for (size_t ti = 0; ti < getTrainDataSize(); ti++)
pre_id_v.at(ti) = m_train_data.at(ti)->getID();
// 2. Calc Center
vi vn(getCenterNum(), 0);
for (size_t ti = 0; ti < getTrainDataSize(); ti++) {
auto train_data_ti = m_train_data.at(ti);
auto id_ti = train_data_ti->getID();
vn.at(id_ti)++;
for (size_t cdi = 0; cdi < (m_train_data.at(ti))->getSize(); cdi++) {
auto data_ti_cdi = train_data_ti->getData(cdi), cluster_ti_cdi = m_center.at(id_ti)->getData(cdi);
m_center.at(id_ti)->setData(cdi, cluster_ti_cdi + data_ti_cdi);
}
}
for (size_t ci = 0; ci < getCenterNum(); ci++) {
if (!vn.at(ci)) {
cerr << "Cluster Size = 0, aborted" << endl;
exit(1);
}
auto center_ci = m_center.at(ci);
for (size_t cdi = 0; cdi < center_ci->getSize(); cdi++) {
center_ci->setData(cdi, center_ci->getData(cdi) / vn.at(ci));
}
}
// 3. Change ID
for (size_t ti = 0; ti < getTrainDataSize(); ti++)
m_train_data.at(ti)->setID(getMinimumClusterID(m_train_data.at(ti)));
// 4. Check if Revision has Occurred
for (size_t ti = 0; ti < getTrainDataSize(); ti++)
if (pre_id_v.at(ti) != m_train_data.at(ti)->getID())
return 1;
return 0;
}
void GetDirectionalDoG(imatrix& image, ETF& e, mymatrix& dog, myvec& GAU1, myvec& GAU2, double tau)
{
myvec vn(2);
double x, y, d_x, d_y;
double weight1, weight2, w_sum1, sum1, sum2, w_sum2;
int s;
int x1, y1;
int i, j;
int dd;
double val;
int half_w1, half_w2;
half_w1 = GAU1.getMax()-1;
half_w2 = GAU2.getMax()-1;
int image_x, image_y;
image_x = image.getRow();
image_y = image.getCol();
for (i = 0; i < image_x; i++) {
for (j = 0; j < image_y; j++) {
sum1 = sum2 = 0.0;
w_sum1 = w_sum2 = 0.0;
weight1 = weight2 = 0.0;
vn[0] = -e[i][j].ty;
vn[1] = e[i][j].tx;
if (vn[0] == 0.0 && vn[1] == 0.0) {
sum1 = 255.0;
sum2 = 255.0;
dog[i][j] = sum1 - tau * sum2;
continue;
}
d_x = i; d_y = j;
////////////////////////////////////////
for (s = -half_w2; s <= half_w2; s++) {
////////////////////////
x = d_x + vn[0] * s;
y = d_y + vn[1] * s;
/////////////////////////////////////////////////////
if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0)
continue;
x1 = round(x); if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
y1 = round(y); if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
val = image[x1][y1];
/////////////////////////////////////////////////////////
dd = ABS(s);
if (dd > half_w1) weight1 = 0.0;
else weight1 = GAU1[dd];
//////////////////////////////////
sum1 += val * weight1;
w_sum1 += weight1;
/////////////////////////////////////////////////////
weight2 = GAU2[dd];
sum2 += val * weight2;
w_sum2 += weight2;
}
/////////////////////////
sum1 /= w_sum1;
sum2 /= w_sum2;
//////////////////////////////////////
dog[i][j] = sum1 - tau * sum2;
}
}
}
//游戏开始
void CTableFrameSink::GameStart()
{
m_oLog.Log("//////-------游戏开始--------////////");
for(int i=0; i<GAME_PLAYER; i++)
{
IServerUserItem * pIServerUserItem = m_pITableFrame->GetServerUserItem(i);
if (pIServerUserItem == NULL) continue;
if (!pIServerUserItem->IsAndroidUser())
{
m_oLog.Log("玩家[%s] 带入筹码[%I64d]" , pIServerUserItem->GetAccounts() , pIServerUserItem->GetUserScore()->lBodyChip);
}
}
CountDownTime(INVALID_CHAIR);
CMD_S_GameStart GameStart;
ZeroMemory(&GameStart,sizeof(GameStart));
//强发聚宝盆
//m_oLog.Log("强发状态：%s",m_bMeetConditions?"已经进行过强发，还未清除状态不能强发！":"未进行过强发，可以强发！");
bool bSuccess = false;//= ForceDispathCard();
//m_oLog.Log("1");
while (!bSuccess)
{
//m_oLog.Log("2");
bSuccess = DispathCard();
}
//随机喜金牌
ZeroMemory(m_cbRewardCardData,sizeof(m_cbRewardCardData));
for (int i=0; i<GAME_PLAYER; i++)
{
if (m_cbPlayStatus[i] == FALSE) continue;
m_GameLogic.RandCardList(m_cbRewardCardData[i],MAX_COUNT);
}
//测试配牌
if (m_bCheat)
{
for (int i=0; i<GAME_PLAYER; i++)
{
if (m_cbCheatCardData[i][0] == 0)continue;
CopyMemory(m_cbHandCardData[i], m_cbCheatCardData[i],MAX_COUNT);
}
for (int i=0; i<GAME_PLAYER; i++)
{
if (m_cbCheatRewardCard[i][0] ==0)continue;
{
CopyMemory(m_cbRewardCardData[i], m_cbCheatRewardCard[i], MAX_COUNT);
}
}
}
m_oLog.Log("原始手牌：");
for(int i=0; i<GAME_PLAYER; i++)
{
//获取用户
IServerUserItem *pIServerUser=m_pITableFrame->GetServerUserItem(i);
if(pIServerUser==NULL) continue;
m_oLog.Log("%s手牌:[%s%s][%s%s][%s%s][%s%s][%s%s]---牌型为[%s]",pIServerUser->GetUserData()->szAccounts,cn(m_cbHandCardData[i][0]),vn(m_cbHandCardData[i][0]),
cn(m_cbHandCardData[i][1]),vn(m_cbHandCardData[i][1]),cn(m_cbHandCardData[i][2]),vn(m_cbHandCardData[i][2]),cn(m_cbHandCardData[i][3]),vn(m_cbHandCardData[i][3]),
cn(m_cbHandCardData[i][4]),vn(m_cbHandCardData[i][4]),CardTypeName(m_GameLogic.GetBestCardType(m_cbHandCardData[i],MAX_COUNT)));
}
m_oContral.ControlDispath(m_cbHandCardData, m_cbPlayStatus);
m_oLog.Log("控制后手牌：");
for(int i=0; i<GAME_PLAYER; i++)
{
//获取用户
IServerUserItem *pIServerUser=m_pITableFrame->GetServerUserItem(i);
if(pIServerUser==NULL) continue;
m_oLog.Log("%s手牌:[%s%s][%s%s][%s%s][%s%s][%s%s]---牌型为[%s]",pIServerUser->GetUserData()->szAccounts,cn(m_cbHandCardData[i][0]),vn(m_cbHandCardData[i][0]),
cn(m_cbHandCardData[i][1]),vn(m_cbHandCardData[i][1]),cn(m_cbHandCardData[i][2]),vn(m_cbHandCardData[i][2]),cn(m_cbHandCardData[i][3]),vn(m_cbHandCardData[i][3]),
cn(m_cbHandCardData[i][4]),vn(m_cbHandCardData[i][4]),CardTypeName(m_GameLogic.GetBestCardType(m_cbHandCardData[i],MAX_COUNT)));
}
//
//发送扑克
for (WORD i=0;i<m_wPlayerCount;i++)
{
if(m_cbPlayStatus[i]==FALSE)continue;
CopyMemory(GameStart.cbPlayStatus, m_cbPlayStatus,sizeof(m_cbPlayStatus));
//派发扑克
CopyMemory(GameStart.cbCardData[i],m_cbHandCardData[i],MAX_COUNT);
std::random_shuffle(GameStart.cbCardData[i], GameStart.cbCardData[i]+MAX_COUNT);
//派发喜金扑克
CopyMemory(GameStart.cbRewardCardData[i], m_cbRewardCardData[i],MAX_COUNT);
//发送数据
m_pITableFrame->SendTableData(i,SUB_S_GAME_START,&GameStart,sizeof(GameStart));
ZeroMemory(GameStart.cbCardData, sizeof(GameStart.cbCardData));
}
m_pITableFrame->SendLookonData(INVALID_CHAIR,SUB_S_GAME_START,&GameStart,sizeof(GameStart));
}
//! \brief Returns closest point on line to given point p.
//!
//! If vector is null (line is not valid) returns _point.
P project(const P& p) const {
V vn(_vector.normalized());
return _point + ((p-_point)*vn) *vn;
}
int main()
{
poc();
try {
TestSection("SVector");
TestSubsection("Matrix");
{
SMatrix<5> matrix;
matrix[1][2][1][0][1] = 123;
matrix[2][1][0][1][4] = 777;
TEST("matrix_1", matrix[1][2][1][0][1].GetInt(), 123);
TEST("matrix_2", matrix[2][1][0][1][4].GetInt(), 777);
}
TestSubsection("Matrix_has_you");
{
SVector m;
SVector m0;
SVector m1;
SVector m00;
SVector m01;
SVector m10;
SVector m11;
m10[1] = SReference(25);
m0[0] = m00;
m0[1] = m01;
m1[0] = m10;
m1[1] = m11;
m[0] = m0;
m[1] = m1;
SMatrixRef<3> matr(m);
TEST("matrix_has_you", matr[1][0][1].GetInt(), 25);
}
TestSubsection("resize");
{
SVector v;
v[7] = 5;
TEST("resize_1", v->Size(), 8);
v[12] = 5;
TEST("resize_2", v->Size(), 13);
v[13] = 5;
TEST("resize_3", v->Size(), 14);
}
#if INTELIB_DEBUG_COUNTERS == 1
TestSubsection("leaks");
{
int before = SExpression::object_counter;
{
SVector v;
v[7] = 5;
SVectorRef vr(v);
SVectorRef vr2;
vr2 = vr;
SVectorRef vr3(vr2);
for(int i=0; i<200; i++) vr3[vr3->Size()] = SReference(i);
}
TEST("no_leaks", SExpression::object_counter, before);
}
#endif
TestSubsection("TextRepresentation");
{
SVector v;
for(int i=0; i<5; i++) v[i]=i;
TEST("text_rep", v->TextRepresentation().c_str(),
"#~(0 1 2 3 4)");
}
TestSubsection("Range Copying");
{
SVector v;
for(int i=0; i<15; i++) v[i]=i;
SVectorRange r(v,5,3);
TEST("range_copy", r.Copy()->TextRepresentation().c_str(),
"#~(5 6 7)");
TESTB("copy_keeps_positive_resizeability",
r.Copy()->IsResizeable());
SVector vn(5);
for(int i=0; i<5; i++) vn[i]=i*100;
SVectorRange rn(vn,1,2);
TESTB("copy_keeps_negative_resizeability",
!rn.Copy()->IsResizeable());
TESTB("copy_positive_resizeability",
rn.Copy(true)->IsResizeable());
TESTB("copy_negative_resizeability",
!rn.Copy(false)->IsResizeable());
SVectorRange r200(v,5,200);
TEST("range_size_limited", r200.Copy()->Size(), 10);
}
TestSubsection("Range Erasing");
{
SVector v;
for(int i=0; i<15; i++) v[i]=i;
SVectorRange r(v,3,10);
r.Erase();
TEST("range_erase", v->TextRepresentation().c_str(),
"#~(0 1 2 13 14)");
TEST("range_erase_size", v->Size(), 5);
TEST("range_erase_range_len", r.Copy()->Size(), 0);
}
TestSubsection("Range Replacing");
{
SVector v, w;
for(int i=0; i<15; i++) v[i]=i;
for(int i=0; i<5; i++) w[i]=i*100;
SVectorRange r(v,3,10);
r.Replace(w);
TEST("range_replace_less", v->TextRepresentation().c_str(),
"#~(0 1 2 0 100 200 300 400 13 14)");
TEST("range_replace_less_size", v->Size(), 10);
TEST("range_replace_less_range_len", r.Copy()->Size(), 5);
}
{
SVector v, w;
for(int i=0; i<10; i++) v[i]=i;
for(int i=0; i<5; i++) w[i]=i*100;
SVectorRange r(w,1,2);
r.Replace(v);
TEST("range_replace_more", w->TextRepresentation().c_str(),
"#~(0 0 1 2 3 4 5 6 7 8 9 300 400)");
TEST("range_replace_more_size", w->Size(), 13);
TEST("range_replace_more_range_len", r.Copy()->Size(), 10);
}
TestScore();
}
catch(const IntelibX &ex) {
printf("Caught IntelibX: %s\n%s\n",
ex.Description(),
ex.Parameter().GetPtr() ?
ex.Parameter()->TextRepresentation().c_str() : "");
}
catch(...) {
printf("Something strange caught\n");
}
poc();
return 0;
}
void PABounce::Execute(ParticleGroup *group)
{
switch(position.type)
{
case PDTriangle:
{
// Compute the inverse matrix of the plane basis.
pVector &u = position.u;
pVector &v = position.v;
// w = u cross v
float wx = u.y*v.z-u.z*v.y;
float wy = u.z*v.x-u.x*v.z;
float wz = u.x*v.y-u.y*v.x;
float det = 1/(wz*u.x*v.y-wz*u.y*v.x-u.z*wx*v.y-u.x*v.z*wy+v.z*wx*u.y+u.z*v.x*wy);
pVector s1((v.y*wz-v.z*wy), (v.z*wx-v.x*wz), (v.x*wy-v.y*wx));
s1 *= det;
pVector s2((u.y*wz-u.z*wy), (u.z*wx-u.x*wz), (u.x*wy-u.y*wx));
s2 *= -det;
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float distold = m.pos * position.p2 + position.radius1;
float distnew = pnext * position.p2 + position.radius1;
// Opposite signs if product < 0
// Is there a faster way to do this?
if(distold * distnew >= 0)
continue;
// Find position at the crossing point by parameterizing
// p(t) = pos + vel * t
// Solve dist(p(t),plane) = 0 e.g.
// n * p(t) + D = 0 ->
// n * p + t (n * v) + D = 0 ->
// t = -(n * p + D) / (n * v)
// Could factor n*v into distnew = distold + n*v and save a bit.
// Safe since n*v != 0 assured by quick rejection test.
// This calc is indep. of dt because we have established that it
// will hit before dt. We just want to know when.
float nv = position.p2 * m.vel;
float t = -distold / nv;
// Actual intersection point p(t) = pos + vel t
pVector phit(m.pos + m.vel * t);
// Offset from origin in plane, p - origin
pVector offset(phit - position.p1);
// Dot product with basis vectors of old frame
// in terms of new frame gives position in uv frame.
float upos = offset * s1;
float vpos = offset * s2;
// Did it cross plane outside triangle?
if(upos < 0 || vpos < 0 || (upos + vpos) > 1)
continue;
// A hit! A most palpable hit!
// Compute tangential and normal components of velocity
pVector vn(position.p2 * nv); // Normal Vn = (V.N)N
pVector vt(m.vel - vn); // Tangent Vt = V - Vn
// Compute new velocity heading out:
// Don't apply friction if tangential velocity < cutoff
if(vt.length2() <= cutoffSqr)
m.vel = vt - vn * resilience;
else
m.vel = vt * oneMinusFriction - vn * resilience;
}
}
break;
case PDDisc:
{
float r1Sqr = fsqr(position.radius1);
float r2Sqr = fsqr(position.radius2);
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d. radius1Sqr stores d.
float distold = m.pos * position.p2 + position.radius1Sqr;
float distnew = pnext * position.p2 + position.radius1Sqr;
// Opposite signs if product < 0
// Is there a faster way to do this?
if(distold * distnew >= 0)
continue;
// Find position at the crossing point by parameterizing
// p(t) = pos + vel * t
// Solve dist(p(t),plane) = 0 e.g.
// n * p(t) + D = 0 ->
// n * p + t (n * v) + D = 0 ->
// t = -(n * p + D) / (n * v)
// Could factor n*v into distnew = distold + n*v and save a bit.
// Safe since n*v != 0 assured by quick rejection test.
// This calc is indep. of dt because we have established that it
// will hit before dt. We just want to know when.
float nv = position.p2 * m.vel;
float t = -distold / nv;
// Actual intersection point p(t) = pos + vel t
pVector phit(m.pos + m.vel * t);
// Offset from origin in plane, phit - origin
pVector offset(phit - position.p1);
float rad = offset.length2();
if(rad > r1Sqr || rad < r2Sqr)
continue;
// A hit! A most palpable hit!
// Compute tangential and normal components of velocity
pVector vn(position.p2 * nv); // Normal Vn = (V.N)N
pVector vt(m.vel - vn); // Tangent Vt = V - Vn
// Compute new velocity heading out:
// Don't apply friction if tangential velocity < cutoff
if(vt.length2() <= cutoffSqr)
m.vel = vt - vn * resilience;
else
m.vel = vt * oneMinusFriction - vn * resilience;
}
}
break;
case PDPlane:
{
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float distold = m.pos * position.p2 + position.radius1;
float distnew = pnext * position.p2 + position.radius1;
// Opposite signs if product < 0
if(distold * distnew >= 0)
continue;
// Compute tangential and normal components of velocity
float nmag = m.vel * position.p2;
pVector vn(position.p2 * nmag); // Normal Vn = (V.N)N
pVector vt(m.vel - vn); // Tangent Vt = V - Vn
// Compute new velocity heading out:
// Don't apply friction if tangential velocity < cutoff
if(vt.length2() <= cutoffSqr)
m.vel = vt - vn * resilience;
else
m.vel = vt * oneMinusFriction - vn * resilience;
}
}
break;
case PDRectangle:
{
// Compute the inverse matrix of the plane basis.
pVector &u = position.u;
pVector &v = position.v;
// w = u cross v
float wx = u.y*v.z-u.z*v.y;
float wy = u.z*v.x-u.x*v.z;
float wz = u.x*v.y-u.y*v.x;
float det = 1/(wz*u.x*v.y-wz*u.y*v.x-u.z*wx*v.y-u.x*v.z*wy+v.z*wx*u.y+u.z*v.x*wy);
pVector s1((v.y*wz-v.z*wy), (v.z*wx-v.x*wz), (v.x*wy-v.y*wx));
s1 *= det;
pVector s2((u.y*wz-u.z*wy), (u.z*wx-u.x*wz), (u.x*wy-u.y*wx));
s2 *= -det;
// See which particles bounce.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's current and next positions cross plane.
// If not, couldn't bounce, so keep going.
pVector pnext(m.pos + m.vel * dt);
// p2 stores the plane normal (the a,b,c of the plane eqn).
// Old and new distances: dist(p,plane) = n * p + d
// radius1 stores -n*p, which is d.
float distold = m.pos * position.p2 + position.radius1;
float distnew = pnext * position.p2 + position.radius1;
// Opposite signs if product < 0
if(distold * distnew >= 0)
continue;
// Find position at the crossing point by parameterizing
// p(t) = pos + vel * t
// Solve dist(p(t),plane) = 0 e.g.
// n * p(t) + D = 0 ->
// n * p + t (n * v) + D = 0 ->
// t = -(n * p + D) / (n * v)
float t = -distold / (position.p2 * m.vel);
// Actual intersection point p(t) = pos + vel t
pVector phit(m.pos + m.vel * t);
// Offset from origin in plane, p - origin
pVector offset(phit - position.p1);
// Dot product with basis vectors of old frame
// in terms of new frame gives position in uv frame.
float upos = offset * s1;
float vpos = offset * s2;
// Crossed plane outside bounce region if !(0<=[uv]pos<=1)
if(upos < 0 || upos > 1 || vpos < 0 || vpos > 1)
continue;
// A hit! A most palpable hit!
// Compute tangential and normal components of velocity
float nmag = m.vel * position.p2;
pVector vn(position.p2 * nmag); // Normal Vn = (V.N)N
pVector vt(m.vel - vn); // Tangent Vt = V - Vn
// Compute new velocity heading out:
// Don't apply friction if tangential velocity < cutoff
if(vt.length2() <= cutoffSqr)
m.vel = vt - vn * resilience;
else
m.vel = vt * oneMinusFriction - vn * resilience;
}
}
break;
case PDSphere:
{
// Sphere that particles bounce off
// The particles are always forced out of the sphere.
for(int i = 0; i < group->p_count; i++)
{
Particle &m = group->list[i];
// See if particle's next position is inside domain.
// If so, bounce it.
pVector pnext(m.pos + m.vel * dt);
if(position.Within(pnext))
{
// See if we were inside on previous timestep.
bool pinside = position.Within(m.pos);
// Normal to surface. This works for a sphere. Isn't
// computed quite right, should extrapolate particle
// position to surface.
pVector n(m.pos - position.p1);
n.normalize();
// Compute tangential and normal components of velocity
float nmag = m.vel * n;
pVector vn(n * nmag); // Normal Vn = (V.N)N
pVector vt = m.vel - vn; // Tangent Vt = V - Vn
if(pinside)
{
// Previous position was inside. If normal component of
// velocity points in, reverse it. This effectively
// repels particles which would otherwise be trapped
// in the sphere.
if(nmag < 0)
m.vel = vt - vn;
}
else
{
// Previous position was outside -> particle will cross
// surface boundary. Reverse normal component of velocity,
// and apply friction (if Vt >= cutoff) and resilience.
// Compute new velocity heading out:
// Don't apply friction if tangential velocity < cutoff
if(vt.length2() <= cutoffSqr)
m.vel = vt - vn * resilience;
else
m.vel = vt * oneMinusFriction - vn * resilience;
}
}
}
}
default:
break;
}
}
Vector2f Vector2f::negate(Vector2f v) {
Vector2f vn(-v.x, -v.y);
return vn;
}
int pick_major_axis(
vector<pair<Pmwx::Halfedge_handle, Pmwx::Halfedge_handle> >& sides, // Per side: inclusive range of half-edges "consolidated" into the sides.
Polygon2& bounds, // Inset boundary in metric, first side matched to the list.
Vector2& v_x,
Vector2& v_y)
{
// special case: if we find a block with exactly ONE right angle, the longer of the two
// sides going into the right angle is hte major axis, full stop, we're done.
int right_angle = -1;
for(int i = 0; i < sides.size(); ++i)
{
int j = (i + 1) % sides.size();
int k = (i + 2) % sides.size();
Vector2 vx(Vector2(bounds[i],bounds[j]));
Vector2 vy(Vector2(bounds[j],bounds[k]));
vx.normalize();
vy.normalize();
double dot = fabs(vx.dot(vy));
if(dot < 0.087155742747658)
{
if(right_angle == -1)
right_angle = j;
else
right_angle = -2; // "more than one right angle" flag - causes us to NOT try this algo.
}
}
if(right_angle >= 0)
{
int prev = (right_angle + sides.size() - 1) % sides.size();
int next = (right_angle + 1) % sides.size();
Vector2 vp(Vector2(bounds[prev],bounds[right_angle]));
Vector2 vn(Vector2(bounds[right_angle],bounds[next]));
double pl = vp.normalize();
double nl = vn.normalize();
if(pl > nl)
v_x = vp;
else
v_x = vn;
v_y = v_x.perpendicular_ccw();
return right_angle;
}
// THIS is the algo we shipped with - it tries to minimize the short side axis of the block. This works
// okay but tends to make the diagonal of diagonal cuts (like Broadway) the main axis since (by the pythag
// theorem) that slightly reduces the block depth.
#if 0
int shortest = -1;
double thinnest_so_far = 0.0;
for(int i = 0; i < sides.size(); ++i)
{
Vector2 vx = Vector2(bounds.side(i).p1,bounds.side(i).p2);
vx.normalize();
Vector2 vy = vx.perpendicular_ccw();
double bbox[4];
bbox[0] = bbox[2] = vx.dot(Vector2(bounds[0]));
bbox[1] = bbox[3] = vy.dot(Vector2(bounds[0]));
for(int j = 0; j < sides.size(); ++j)
{
double x = vx.dot(Vector2(bounds[j]));
double y = vy.dot(Vector2(bounds[j]));
bbox[0] = dobmin2(bbox[0], x);
bbox[1] = dobmin2(bbox[1], y);
bbox[2] = dobmax2(bbox[2], x);
bbox[3] = dobmax2(bbox[3], y);
}
double xdist = fabs(bbox[2]-bbox[0]);
double ydist = fabs(bbox[3]-bbox[1]);
double my_dist = dobmin2(xdist,ydist);
if(shortest == -1 || my_dist < thinnest_so_far)
{
shortest = i;
thinnest_so_far = my_dist;
if(xdist < ydist)
{
v_x = vx.perpendicular_ccw();
v_y = vy.perpendicular_ccw();
}
else
{
v_x = vx;
v_y = vy;
}
}
}
DebugAssert(shortest >= 0);
return shortest;
#endif
#if 1
// #error This algo works 95% of the time, but 5% of the time it picks a slashed short end as the
// #error long axis, which gives a long thin block a wrong axis alignment and a huge AABB. Bad!
// The basic idea: we want to pick the grid axis MOST aligned with the block such that
// the major axis supports roads.
double best_corr = 0;
double best = -1;
Vector2 best_vec;
bool elev_ok = false;
int i, j, tries;
for(tries = 0; tries < 2; ++tries)
{
for(i = 0; i < sides.size(); ++i)
if(elev_ok || ground_road_access_for_he(sides[i].first))
{
double score = 0.0;
Vector2 si = Vector2(bounds.side(i).p1,bounds.side(i).p2);
si.normalize();
Vector2 si_n = si.perpendicular_ccw();
for(int j = 0; j < sides.size(); ++j)
if(elev_ok || ground_road_access_for_he(sides[j].first))
{
Vector2 sj(bounds.side(j).p1,bounds.side(j).p2);
double my_corr = fltmax2(fabs(si.dot(sj)),fabs(si_n.dot(sj)));
score += my_corr;
}
if(score > best_corr){
best = i;
best_corr = score;
best_vec = si;
}
}
if(best >= 0)
break;
elev_ok = true;
}
if(best >= 0)
{
Vector2 best_vec_n = best_vec.perpendicular_ccw();
int longest = -1;
double corr_len = -1;
for(int i = 0; i < sides.size(); ++i)
if(/*elev_ok ||*/ ground_road_access_for_he(sides[i].first))
{
Vector2 this_side(bounds.side(i).p1,bounds.side(i).p2);
double len = this_side.normalize();
double my_corr = fltmax2(fabs(best_vec.dot(this_side)), fabs(best_vec_n.dot(this_side)));
if(my_corr > 0.996194698091746)
{
my_corr *= len;
if(my_corr > corr_len)
{
longest = i;
corr_len = my_corr;
}
}
}
if(longest >= 0)
best = longest;
}
v_x = Vector2(bounds.side(best).p1,bounds.side(best).p2);
v_x.normalize();
v_y = v_x.perpendicular_ccw();
//printf("So far our best is %d, with axes %lf, %lf to %lf, %lf\n", best, v_x.dx,v_x.dy,v_y.dx,v_y.dy);
double bbox[4];
bbox[0] = bbox[2] = v_x.dot(Vector2(bounds[0]));
bbox[1] = bbox[3] = v_y.dot(Vector2(bounds[0]));
for(i = 1; i < sides.size(); ++i)
{
double va = v_x.dot(Vector2(bounds[i]));
double vb = v_y.dot(Vector2(bounds[i]));
bbox[0]=dobmin2(bbox[0], va);
bbox[1]=dobmin2(bbox[1], vb);
bbox[2]=dobmax2(bbox[2], va);
bbox[3]=dobmax2(bbox[3], vb);
}
//printf("Our bbox is %lf,%lf to %lf,%lf\n", bbox[0],bbox[1],bbox[2],bbox[3]);
if(0)
if((bbox[2] - bbox[0]) < (bbox[3] - bbox[1]))
{
v_x = v_x.perpendicular_ccw();
v_y = v_y.perpendicular_ccw();
//printf("Must rotate, the winner was a SHORT side.\n");
}
// best = 0;
// double best_dot = 0;
// for(i = 0; i < sides.size(); ++i)
// if(ground_road_access_for_he(sides[i].first))
// {
// Vector2 side_vec(bounds.side(i).p1,bounds.side(i).p2);
// double slen = side_vec.normalize();
// double corr = fabs(v_x.dot(side_vec));
// if(corr > best_dot)
// {
// best = i;
// corr = best_dot;
// }
// }
// DebugAssert(best >= 0.0);
// v_x = Vector2(bounds.side(best).p1,bounds.side(best).p2);
// v_x.normalize();
// v_y = v_x.perpendicular_ccw();
return best;
#endif
}
bool objMesh::parseFile(std::string fname){
std::ifstream f;
f.open(fname.c_str());
// Check if it opened
if (!f.is_open()){
std::cout << "\nCould not open file " << fname << ".\n";
return false;
}
else {
//std::cout << "Opening " << fname << std::endl;
}
// Temporary containers for structs
std::vector<face> faces;
std::vector<vertex> verts;
std::vector<vector3> norms;
std::vector<texture> texts;
// Parse
std::string line;
while (getline(f,line) )
{
float x, y, z;
char type[3];
int n = sscanf(line.c_str(), "%s %f %f %f", type, &x, &y, &z);
// Comment/blank
if (strcmp(type, "#") == 0 || n <= 0)
continue;
// Vertex
else if (strcmp(type, "v") == 0 && n == 4){
vertex v(x, y, z);
verts.push_back(v);
}
//Vertex normal
else if (strcmp(type, "vn")==0){
// normalize
float mag = sqrtf(x*x + y*y + z*z);
vector3 vn(x/mag, y/mag, z/mag);
norms.push_back(vn);
}
// Texture
else if (strcmp(type, "vt") == 0 && (n == 3 || n == 4)){
texture t = {n == 4, x, y, z};
texts.push_back(t);
}
// Face
else if (strcmp(type, "f") == 0){
bool normalIncluded = false;
// Get textures
float tx = 0, ty = 0, tz = 0, nx = 0, ny = 0, nz = 0;
// Slashes used
if (n == 2) {
n = sscanf(line.c_str(), "%s %f/%f %f/%f %f/%f", type, &x, &tx, &y, &ty, &z, &tz);
// Check if double slashes used
if (n < 7) {
n = sscanf(line.c_str(), "%s %f//%f %f//%f %f//%f", type, &x, &tx, &y, &ty, &z, &tz);
// Check if normals included
if (n < 7) {
n = sscanf(line.c_str(), "%s %f/%f/%f %f/%f/%f %f/%f/%f",
type, &x, &tx, &nx, &y, &ty, &ny, &z, &tz, &nz);
if (n != 10) {
std::cout << "Could not parse: " << line << std::endl;
}
normalIncluded = true;
}
}
}
// Create face
face f;
// Add vertices
assert(!(x > verts.size() || y > verts.size() || z >verts.size()));
f.vert[0] = verts[x - 1];
f.vert[1] = verts[y - 1];
f.vert[2] = verts[z - 1];
// Add normals
if (!norms.empty() && normalIncluded){
f.norm[0] = norms[nx - 1];
f.norm[1] = norms[ny - 1];
f.norm[2] = norms[nz - 1]; }
else {
vector3 edge_1 = f.vert[1] - f.vert[0];
vector3 edge_2 = f.vert[2] - f.vert[0];
vector3 n = cross(edge_1, edge_2);
for (size_t i = 0; i < 3; i++) f.norm[i] = n;
}
// Textures (?)
f.textured = !(n == 4);
if (f.textured){
assert(!(tx > texts.size() || ty > texts.size() || tz >texts.size()));
f.text[0] = texts[tx - 1];
f.text[1] = texts[ty - 1];
f.text[2] = texts[tz - 1];
}
faces.push_back(f);
}
else {
std::cout << "Could not parse: " << line << std::endl;
}
}
f.close();
nVertices = faces.size() * 3.0;
// Store to float arrays
vertices = new float[nVertices * 3];
normals = new float[nVertices * 3];
textureCoords = new float[nVertices * 2];
size_t array_counter = 0;
size_t tex_counter = 0;
for(std::vector<face>::iterator f = faces.begin(); f != faces.end(); f++){
for (size_t vert = 0; vert < 3; vert++){
float normSquared = 0;
for (size_t d = 0; d < 3; d++){
// Update normSquare
normSquared += powf(f->vert[vert][d], 2.0);
vertices[array_counter] = f->vert[vert][d];
normals[array_counter] = f->norm[vert][d];
array_counter++;
if (f->textured && d < 2){
textureCoords[tex_counter] = f->text[vert][d];
tex_counter++;
}
#ifdef DEBUG_TESS
std::cout << "NORMAL x: " << normals[array_counter - 3] << " Y: ";
std::cout << normals[array_counter - 2] << " Z: ";
std::cout << normals[array_counter - 1] << std::endl;
#endif
}
// Update radius
if (normSquared > radius)
radius = normSquared;
}
}
// Radius is currently squared
radius = powf(radius, 0.5);
return true;
}
//need better algo
int main(){
vector<int> vinit;
vinit.resize(10,0);
i64 max = 0;
for(int i = 0; i <= 9; ++i)
vinit[i] = 9 - i; //init to 9876543210
for(int n = 9; n<=99; n+=1){
vector<int> vn(vinit);
vector<int> flags;
flags.resize(10,0);
int n1 = n;
bool accept = true;
while(n1){
int res = n1 % 10;
if(flags[res]==0)
flags[res] = 1;
else{
accept = false;
break;
}
n1/= 10;
}
if(!accept) continue;
vector<int> f1(flags);
do{
i64 res1 = 0;
for(unsigned int i = 0; i<vn.size(); ++i){
res1 *= 10;
res1 += vn[i];
}
if(res1 < 9786105234LL) break;
i64 tp = 0;
bool failed = false;
int nb = 0;
for(unsigned int i = 0; i< vn.size(); ++i){
tp = tp * 10 + vn[i];
if(i == vn.size()-1 && tp == 0 && f1[0] == 1)
failed = true;
if(tp % n == 0){
++nb;
tp /= n;
while(tp){
int res = tp % 10;
tp /= 10;
if(f1[res] == 0)
f1[res] = 1;
else{
failed = true;
break;
}
}
tp = 0;
}
if(failed)
break;
}
if(tp != 0)
failed = true;
int sum = accumulate(f1.begin(), f1.end(), 0);
if(nb == 2 && sum ==9 && f1[0] == 0)
failed = true;
if(!(sum == 10|| (sum == 9 && f1[0]==0)))
failed = true;
if(!failed){
i64 res = 0;
for(unsigned int i = 0; i<vn.size(); ++i){
res *= 10;
res += vn[i];
}
if(res > max){
max = res;
printf("%lld %d\n", max, n);
}
break;
}else{
f1 = flags;
}
}while(prev_permutation(vn.begin(), vn.end()));
}
printf("%lld\n", max);
}
void
SubsetViewerPluginInfo::PrivateSetPlotAtts(AttributeSubject *atts,
const ViewerPlot *plot)
{
SubsetAttributes *subsetAtts = (SubsetAttributes *)atts;
//
// Get the meta-data and initialize the subset names and colors in the
// new SubsetAttributes object.
//
const avtDatabaseMetaData *md = plot->GetMetaData();
if (md == NULL)
{
return;
}
avtDatabaseMetaData *nonConstmd = const_cast <avtDatabaseMetaData *>(md);
std::string vn(plot->GetVariableName());
const avtMaterialMetaData *mat = NULL;
const avtScalarMetaData *smd = NULL;
std::string meshName = nonConstmd->MeshForVar(vn);
avtMeshMetaData *mesh =
const_cast <avtMeshMetaData *> (md->GetMesh(meshName));
stringVector sv;
stringVector::const_iterator pos;
std::set<int> groupSet;
std::vector<int> gIDS;
char temp[512];
//
// Create subset names, based on Subset Type
//
avtSubsetType subT = nonConstmd->DetermineSubsetType(vn);
switch (subT)
{
case AVT_DOMAIN_SUBSET :
debug5 << "Variable for subset plot is a domain Mesh." << endl;
subsetAtts->SetSubsetType(SubsetAttributes::Domain);
defaultAtts->SetSubsetType(SubsetAttributes::Domain);
if (mesh->blockNames.empty())
{
for (int i = 0; i < mesh->numBlocks; i++)
{
sprintf(temp, "%d", i+mesh->blockOrigin);
sv.push_back(temp);
}
}
else
{
for(pos = mesh->blockNames.begin();
pos != mesh->blockNames.end(); ++pos)
{
sv.push_back(*pos);
}
}
break;
case AVT_GROUP_SUBSET :
debug5 << "Variable for subset plot is a group Mesh." << endl;
subsetAtts->SetSubsetType(SubsetAttributes::Group);
defaultAtts->SetSubsetType(SubsetAttributes::Group);
if (!mesh->groupNames.empty())
{
for (size_t i = 0; i < mesh->groupNames.size(); ++i)
{
sv.push_back(mesh->groupNames[i]);
}
}
else if (mesh->groupIds.size() > 0)
{
for (size_t i = 0; i < mesh->groupIds.size(); i++)
{
if (groupSet.count(mesh->groupIds[i]) == 0)
{
groupSet.insert(mesh->groupIds[i]);
gIDS.push_back(mesh->groupIds[i]);
}
}
for (size_t i = 0; i < gIDS.size(); i++)
{
sprintf(temp, "%d", gIDS[i]);
sv.push_back(temp);
}
}
else
{
int origin = mesh->groupOrigin;
int nGroups = (int)mesh->groupIdsBasedOnRange.size()-1;
for (int i = 0; i < nGroups; i++)
{
groupSet.insert(origin+i);
gIDS.push_back(origin+i);
sprintf(temp, "%d", origin+i);
sv.push_back(temp);
}
}
break;
case AVT_MATERIAL_SUBSET :
debug5 << "Variable for subset plot is a Material." << endl;
subsetAtts->SetSubsetType(SubsetAttributes::Material);
defaultAtts->SetSubsetType(SubsetAttributes::Material);
mat = md->GetMaterial(vn);
if (mat != NULL)
{
for(pos = mat->materialNames.begin();
pos != mat->materialNames.end(); ++pos)
{
sv.push_back(*pos);
}
}
break;
case AVT_ENUMSCALAR_SUBSET :
debug5 << "Variable for subset plot is an enumerated Scalar."<<endl;
subsetAtts->SetSubsetType(SubsetAttributes::EnumScalar);
defaultAtts->SetSubsetType(SubsetAttributes::EnumScalar);
smd = md->GetScalar(vn);
if (smd != NULL)
{
for(pos = smd->enumNames.begin();
pos != smd->enumNames.end(); ++pos)
{
sv.push_back(*pos);
}
}
break;
default:
if (vn == meshName)
{
debug5 << "Variable for subset plot is a mesh."<<endl;
subsetAtts->SetSubsetType(SubsetAttributes::Mesh);
defaultAtts->SetSubsetType(SubsetAttributes::Mesh);
sprintf(temp, "Whole mesh (%s)", vn.c_str());
sv.push_back(temp);
}
else
{
EXCEPTION1(InvalidVariableException, vn);
}
break;
}
//
// Add a color for each subset name.
//
ColorAttribute *ca = new ColorAttribute[sv.size() + 1];
avtColorTables *ct = avtColorTables::Instance();
if(ct->IsDiscrete(ct->GetDefaultDiscreteColorTable()))
{
// The CT is discrete, get its color color control points.
for(size_t i = 0; i < sv.size(); ++i)
{
unsigned char rgb[3] = {0,0,0};
ct->GetControlPointColor(ct->GetDefaultDiscreteColorTable(), (int)i, rgb);
ca[i].SetRed(int(rgb[0]));
ca[i].SetGreen(int(rgb[1]));
ca[i].SetBlue(int(rgb[2]));
}
}
else
{
// The CT is continuous, sample the CT so we have a unique color
// for each element in sv.
unsigned char *rgb = ct->GetSampledColors(
ct->GetDefaultDiscreteColorTable(), (int)sv.size());
if(rgb)
{
for(size_t i = 0; i < sv.size(); ++i)
{
ca[i].SetRed(int(rgb[i*3]));
ca[i].SetGreen(int(rgb[i*3+1]));
ca[i].SetBlue(int(rgb[i*3+2]));
}
delete [] rgb;
}
}
ColorAttributeList cal;
int idx = 0;
for(pos = sv.begin(); pos != sv.end(); ++pos)
{
if (idx < subsetAtts->GetMultiColor().GetNumColors())
{
// The meshIndex is within the defaultAtts' color
// vector size.
cal.AddColors(subsetAtts->GetMultiColor()[idx]);
}
else
{
// The meshIndex is greater than the size of the
// defaultAtts' color vector. Use colors from the
// default discrete color table.
cal.AddColors(ca[idx]);
}
++idx;
}
delete [] ca;
// Set the subset names and colors in the subsetAtts.
subsetAtts->SetSubsetNames(sv);
subsetAtts->SetMultiColor(cal);
defaultAtts->SetSubsetNames(sv);
defaultAtts->SetMultiColor(cal);
}
static DF2(xn2){RZ(a&&w); R vn(xd(a ,w,self));}
//=======================================================================
// profile
// command to build a profile
//=======================================================================
Sketcher_Profile::Sketcher_Profile(const char* aCmd)
{
enum {line, circle, point, none} move;
Standard_Integer i = 1;
Standard_Real x0, y0, x, y, dx, dy;
x0 = y0 = x = y = dy = 0;
dx = 1;
Standard_Boolean first, stayfirst, face, close;
first = Standard_True;
stayfirst = face = close = Standard_False;
Standard_Integer reversed = 0;
Standard_Integer control_Tolerance = 0;
TopoDS_Shape S;
TopoDS_Vertex MP;
BRepBuilderAPI_MakeWire MW;
gp_Ax3 DummyHP(gp::XOY());
gp_Pln P(DummyHP);
TopLoc_Location TheLocation;
Handle(Geom_Surface) Surface;
myOK = Standard_False;
myError = 0;
//TCollection_AsciiString aCommand(CORBA::string_dup(aCmd));
TCollection_AsciiString aCommand ((char*)aCmd);
TCollection_AsciiString aToken = aCommand.Token(":", 1);
int n = 0;
// porting to WNT
TColStd_Array1OfAsciiString aTab (0, aCommand.Length() - 1);
if ( aCommand.Length() )
{
while(aToken.Length() != 0) {
if(aCommand.Token(":", n + 1).Length() > 0)
aTab(n) = aCommand.Token(":", n + 1);
aToken = aCommand.Token(":", ++n);
}
n = n - 1;
}
if ( aTab.Length() && aTab(0).Length() )
while(i < n) {
Standard_Real length = 0, radius = 0, angle = 0;
move = point;
int n1 = 0;
TColStd_Array1OfAsciiString a (0, aTab(0).Length());
aToken = aTab(i).Token(" ", 1);
while (aToken.Length() != 0) {
if (aTab(i).Token(" ", n1 + 1).Length() > 0)
a(n1) = aTab(i).Token(" ", n1 + 1);
aToken = aTab(i).Token(" ", ++n1);
}
n1 = n1 - 1;
switch(a(0).Value(1))
{
case 'F':
{
if (n1 != 3) goto badargs;
if (!first) {
MESSAGE("profile : The F instruction must precede all moves");
return;
}
x0 = x = a(1).RealValue();
y0 = y = a(2).RealValue();
stayfirst = Standard_True;
break;
}
case 'O':
{
if (n1 != 4) goto badargs;
P.SetLocation(gp_Pnt(a(1).RealValue(), a(2).RealValue(), a(3).RealValue()));
stayfirst = Standard_True;
break;
}
case 'P':
{
if (n1 != 7) goto badargs;
gp_Vec vn(a(1).RealValue(), a(2).RealValue(), a(3).RealValue());
gp_Vec vx(a(4).RealValue(), a(5).RealValue(), a(6).RealValue());
if (vn.Magnitude() <= Precision::Confusion() || vx.Magnitude() <= Precision::Confusion()) {
MESSAGE("profile : null direction");
return;
}
gp_Ax2 ax(P.Location(), vn, vx);
P.SetPosition(ax);
stayfirst = Standard_True;
break;
}
case 'X':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (a(0) == "XX")
length -= x;
dx = 1; dy = 0;
move = line;
break;
}
case 'Y':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (a(0) == "YY")
length -= y;
dx = 0; dy = 1;
move = line;
break;
}
case 'L':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (Abs(length) > Precision::Confusion())
move = line;
else
move = none;
break;
}
case 'T':
{
if (n1 != 3) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
if (a(0) == "TT") {
vx -= x;
vy -= y;
}
length = Sqrt(vx * vx + vy * vy);
if (length > Precision::Confusion()) {
move = line;
dx = vx / length;
dy = vy / length;
}
else
move = none;
break;
}
case 'R':
{
if (n1 != 2) goto badargs;
angle = a(1).RealValue() * PI180;
if (a(0) == "RR") {
dx = Cos(angle);
dy = Sin(angle);
}
else {
Standard_Real c = Cos(angle);
Standard_Real s = Sin(angle);
Standard_Real t = c * dx - s * dy;
dy = s * dx + c * dy;
dx = t;
}
break;
}
case 'D':
{
if (n1 != 3) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
length = Sqrt(vx * vx + vy * vy);
if (length > Precision::Confusion()) {
dx = vx / length;
dy = vy / length;
}
else
move = none;
break;
}
case 'C':
{
if (n1 != 3) goto badargs;
radius = a(1).RealValue();
if (Abs(radius) > Precision::Confusion()) {
angle = a(2).RealValue() * PI180;
move = circle;
}
else
move = none;
break;
}
case 'A': // TAngential arc by end point
{
if (n1 != 3) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
if (a(0) == "AA") {
vx -= x;
vy -= y;
}
Standard_Real det = dx * vy - dy * vx;
if ( Abs(det) > Precision::Confusion()) {
Standard_Real c = (dx * vx + dy * vy)
/ Sqrt((dx * dx + dy * dy) * (vx * vx + vy * vy)); // Cosine of alpha = arc of angle / 2 , alpha in [0,Pi]
radius = (vx * vx + vy * vy)* Sqrt(dx * dx + dy * dy) // radius = distance between start and end point / 2 * sin(alpha)
/ (2.0 * det); // radius is > 0 or < 0
if (Abs(radius) > Precision::Confusion()) {
angle = 2.0 * acos(c); // angle in [0,2Pi]
move = circle;
}
else
move = none;
break;
}
else
move = none;
break;
}
case 'U': // Arc by end point and radiUs
{
if (n1 != 5) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
radius = a(3).RealValue();
reversed = a(4).IntegerValue();
if (a(0) == "UU") { // Absolute
vx -= x;
vy -= y;
}
Standard_Real length = Sqrt(vx * vx + vy * vy);
if ( (4.0 - (vx * vx + vy * vy) / (radius * radius) >= 0.0 ) && (length > Precision::Confusion()) ) {
Standard_Real c = 0.5 * Sqrt(4.0 - (vx * vx + vy * vy) / (radius * radius)); // Cosine of alpha = arc angle / 2 , alpha in [0,Pi/2]
angle = 2.0 * acos(c); // angle in [0,Pi]
if ( reversed == 2 )
angle = angle - 2 * PI;
dx = 0.5 * ( vy * 1.0/radius
+ vx * Sqrt(4.0 / (vx * vx + vy * vy) - 1.0 / (radius * radius)));
dy = - 0.5 * ( vx * 1.0/radius
- vy * Sqrt(4.0 / (vx * vx + vy * vy) - 1.0 / (radius * radius)));
move = circle;
}
else{
move = none;
}
break;
}
case 'E': // Arc by end point and cEnter
{
if (n1 != 7) goto badargs;
Standard_Real vx = a(1).RealValue();
Standard_Real vy = a(2).RealValue();
Standard_Real vxc = a(3).RealValue();
Standard_Real vyc = a(4).RealValue();
reversed = a(5).IntegerValue();
control_Tolerance = a(6).IntegerValue();
if (a(0) == "EE") { // Absolute
vx -= x;
vy -= y;
vxc -= x;
vyc -= y;
}
radius = Sqrt( vxc * vxc + vyc * vyc );
Standard_Real det = vx * vyc - vy * vxc;
Standard_Real length = Sqrt(vx * vx + vy * vy);
Standard_Real length2 = Sqrt((vx-vxc) * (vx-vxc) + (vy-vyc) * (vy-vyc));
Standard_Real length3 = Sqrt(vxc * vxc + vyc * vyc);
Standard_Real error = Abs(length2 - radius);
myError = error;
if ( error > Precision::Confusion() ){
MESSAGE("Warning : The specified end point is not on the Arc, distance = "<<error);
}
if ( error > Precision::Confusion() && control_Tolerance == 1) // Don't create the arc if the end point
move = none; // is too far from it
else if ( (length > Precision::Confusion()) &&
(length2 > Precision::Confusion()) &&
(length3 > Precision::Confusion()) ) {
Standard_Real c = ( radius * radius - (vx * vxc + vy * vyc) )
/ ( radius * Sqrt((vx-vxc) * (vx-vxc) + (vy-vyc) * (vy-vyc)) ) ; // Cosine of arc angle
angle = acos(c); // angle in [0,Pi]
if ( reversed == 2 )
angle = angle - 2 * PI;
if (det < 0)
angle = -angle;
dx = vyc / radius;
dy = -vxc / radius;
move = circle;
}
else {
move = none;
}
break;
}
case 'I':
{
if (n1 != 2) goto badargs;
length = a(1).RealValue();
if (a(0) == "IX") {
if (Abs(dx) < Precision::Confusion()) {
MESSAGE("profile : cannot intersect, arg "<<i-1);
return;
}
length = (length - x) / dx;
}
else if (a(0) == "IY") {
if (Abs(dy) < Precision::Confusion()) {
MESSAGE("profile : cannot intersect, arg "<<i-1);
return;
}
length = (length - y) / dy;
}
if (Abs(length) > Precision::Confusion())
move = line;
else
move = none;
break;
}
case 'W':
{
if (a(0) == "WW")
close = Standard_True;
else if(a(0) == "WF") {
close = Standard_True;
face = Standard_True;
}
i = n - 1;
break;
}
default:
{
MESSAGE("profile : unknown code " << a(i));
return;
}
}
again :
switch (move)
{
case line :
{
if (length < 0) {
length = -length;
dx = -dx;
dy = -dy;
}
Handle(Geom2d_Line) l = new Geom2d_Line(gp_Pnt2d(x,y),gp_Dir2d(dx,dy));
BRepBuilderAPI_MakeEdge ME (GeomAPI::To3d(l,P),0,length);
if (!ME.IsDone())
return;
MW.Add(ME);
x += length*dx;
y += length*dy;
break;
}
case circle :
{
Standard_Boolean sense = Standard_True;
if (radius < 0) {
radius = -radius;
sense = !sense;
dx = -dx;
dy = -dy;
}
gp_Ax2d ax(gp_Pnt2d(x-radius*dy,y+radius*dx),gp_Dir2d(dy,-dx));
if (angle < 0) {
angle = -angle;
sense = !sense;
}
Handle(Geom2d_Circle) c = new Geom2d_Circle(ax,radius,sense);
BRepBuilderAPI_MakeEdge ME (GeomAPI::To3d(c,P),0,angle);
if (!ME.IsDone())
return;
MW.Add(ME);
gp_Pnt2d p;
gp_Vec2d v;
c->D1(angle,p,v);
x = p.X();
y = p.Y();
dx = v.X() / radius;
dy = v.Y() / radius;
break;
}
case point:
{
MP = BRepBuilderAPI_MakeVertex(gp_Pnt(x, y, 0.0));
break;
}
case none:
{
i = n - 1;
break;
}
}
// update first
first = stayfirst;
stayfirst = Standard_False;
if(!(dx == 0 && dy == 0))
myLastDir.SetCoord(dx, dy, 0.0);
else
return;
myLastPoint.SetX(x);
myLastPoint.SetY(y);
// next segment....
i++;
if ((i == n) && close) {
// the closing segment
dx = x0 - x;
dy = y0 - y;
length = Sqrt(dx * dx + dy * dy);
move = line;
if (length > Precision::Confusion()) {
dx = dx / length;
dy = dy / length;
goto again;
}
}
}
// get the result, face or wire
if (move == none) {
return;
} else if (move == point) {
S = MP;
} else if (face) {
if (!MW.IsDone()) {
return;
}
BRepBuilderAPI_MakeFace MF (P, MW.Wire());
if (!MF.IsDone()) {
return;
}
S = MF;
} else {
if (!MW.IsDone()) {
return;
}
S = MW;
}
if(!TheLocation.IsIdentity())
S.Move(TheLocation);
myShape = S;
myOK = true;
return;
badargs :
MESSAGE("profile : bad number of arguments");
return;
}
TerrainPatch::TerrainPatch(const ion::base::String& identifier,ion::scene::Renderer& rRenderer,
const ion_uint16 *pHeightdata,const ion_uint32 width,const ion_uint32 pitch,const ion_uint32 depth,
const bool lastrow,const ion::math::Vector3f& offset,const ion::math::Vector3f& size):Renderable(identifier),
m_pTex(0)
{
m_pRenderjob=new ion::scene::Renderjob(*this);
ion::video::Vertexformat vf;
vf.addEntry(ion::video::VertexFormatEntry_Position,ion::video::VertexFormatSemantic_Position);
vf.addEntry(ion::video::VertexFormatEntry_Normal,ion::video::VertexFormatSemantic_Normal);
vf.addEntry(ion::video::VertexFormatEntry_Texcoord2D,ion::video::VertexFormatSemantic_Texcoord);
m_pVertices=rRenderer.videodevice()->createVertexstream(width*depth,vf,ion::video::Streamflags_Writeonly,ion::video::Mempool_Managed);
m_pIndices=rRenderer.videodevice()->createIndexstream((width-1)*(depth-1)*6,
(m_pVertices->capacity()>65536) ? ion::video::Indexformat_32bit : ion::video::Indexformat_16bit,
ion::video::Streamflags_Writeonly,ion::video::Mempool_Managed);
ion_uint32 x,z;
position(offset);
m_BoundingSphere.center(ion::math::Vector3f(size.x()*0.5f,0,size.z()*0.5f));
m_BoundingSphere.radius(ion::math::Vector3f(size.x()*0.5f,size.y(),size.z()*0.5f).length());
m_pVertices->map(ion::video::Map_Writeonly);
ion::video::Vertexiterator v(*m_pVertices);
for (z=0;z<depth;++z) {
for (x=0;x<width;++x) {
float xx=((float)x)/((float)(width-1))*size.x();
float zz=((float)z)/((float)(depth-1))*size.z();
float xx2=((float)(x+1))/((float)(width-1))*size.x();
float zz2=((float)(z+1))/((float)(depth-1))*size.z();
ion_uint32 x_1=x;
ion_uint32 z_1=z;
ion_uint32 x_2=(x+1);
ion_uint32 z_2=(z+1);
ion_uint16 h=pHeightdata[x_1+z_1*pitch];
ion_uint16 h21=pHeightdata[x_2+z_1*pitch];
ion_uint16 h12=pHeightdata[x_1+z_2*pitch];
float yy=((float)h)/65535.0f*size.y();
float yy21=((float)h21)/65535.0f*size.y();
float yy12=((float)h12)/65535.0f*size.y();
{
ion::math::Vector3f vn(xx,yy,zz);
ion::math::Vector3f vn21(xx2,yy21,zz);
ion::math::Vector3f vn12(xx,yy12,zz2);
ion::math::Vector3f n1((vn21-vn).normalizedVector());
ion::math::Vector3f n2((vn12-vn).normalizedVector());
v.normal((n2^n1).normalizedVector());
}
float tu=((float)(x*4))/((float)(width-1));
float tv=((float)(z*4))/((float)(depth-1));
v.position(xx,yy,zz);
v.texcoord2D(0,tu,tv);
++v;
}
}
m_pVertices->unmap();
m_pIndices->map(ion::video::Map_Writeonly);
ion::video::Indexiterator i(*m_pIndices);
for (z=0;z<(depth-1);++z) {
for (x=0;x<(width-1);++x) {
ion_uint32 i1=x+z*width;
ion_uint32 i2=x+1+z*width;
ion_uint32 i3=x+(z+1)*width;
ion_uint32 i4=x+1+(z+1)*width;
i=i1; ++i;
i=i3; ++i;
i=i2; ++i;
i=i2; ++i;
i=i3; ++i;
i=i4; ++i;
}
}
m_pIndices->unmap();
m_Properties.addRef();
m_Properties.addRef();
m_Properties.add2DTexture("diffuseTexture",m_pTex);
m_Properties.addBool("wireframe",false);
m_pRenderjob->firstElement(0);
m_pRenderjob->indexOffset(0);
m_pRenderjob->numElements(m_pIndices->capacity()/3);
m_pRenderjob->primitivestype(ion::video::Primitives_Triangles);
m_pRenderjob->worldtransform()=transform();
m_pRenderjob->indexstream(m_pIndices);
m_pRenderjob->vertexstream(*m_pVertices);
m_pRenderjob->propertytable(&m_Properties);
}
/*
* EKF Attitude Estimator main function.
*
* Estimates the attitude recursively once started.
*
* @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments
*/
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{
float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
float x_aposteriori_k[12]; /**< states */
float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
float x_aposteriori[12];
float P_aposteriori[144];
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
float debugOutput[4] = { 0.0f };
/* Initialize filter */
AttitudeEKF_initialize();
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
gps.eph = 100000;
gps.epv = 100000;
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
uint64_t last_vel_t = 0;
/* current velocity */
math::Vector<3> vel;
vel.zero();
/* previous velocity */
math::Vector<3> vel_prev;
vel_prev.zero();
/* actual acceleration (by GPS velocity) in body frame */
math::Vector<3> acc;
acc.zero();
/* rotation matrix */
math::Matrix<3, 3> R;
R.identity();
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* subscribe to GPS */
int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
/* subscribe to GPS */
int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to control mode */
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* subscribe to vision estimate */
int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
/* subscribe to mocap data */
int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
thread_running = true;
/* keep track of sensor updates */
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params;
memset(&ekf_params, 0, sizeof(ekf_params));
struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
/* magnetic declination, in radians */
float mag_decl = 0.0f;
/* rotation matrix for magnetic declination */
math::Matrix<3, 3> R_decl;
R_decl.identity();
struct vision_position_estimate_s vision {};
struct att_pos_mocap_s mocap {};
/* register the perf counter */
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
/* Main loop*/
while (!thread_should_exit) {
px4_pollfd_struct_t fds[2];
fds[0].fd = sub_raw;
fds[0].events = POLLIN;
fds[1].fd = sub_params;
fds[1].events = POLLIN;
int ret = px4_poll(fds, 2, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) {
warnx("WARNING: Not getting sensor data - sensor app running?");
}
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update);
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
}
/* only run filter if sensor values changed */
if (fds[0].revents & POLLIN) {
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
bool gps_updated;
orb_check(sub_gps, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
}
}
bool global_pos_updated;
orb_check(sub_global_pos, &global_pos_updated);
if (global_pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
}
if (!initialized) {
// XXX disabling init for now
initialized = true;
// gyro_offsets[0] += raw.gyro_rad_s[0];
// gyro_offsets[1] += raw.gyro_rad_s[1];
// gyro_offsets[2] += raw.gyro_rad_s[2];
// offset_count++;
// if (hrt_absolute_time() - start_time > 3000000LL) {
// initialized = true;
// gyro_offsets[0] /= offset_count;
// gyro_offsets[1] /= offset_count;
// gyro_offsets[2] /= offset_count;
// }
} else {
perf_begin(ekf_loop_perf);
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
update_vect[0] = 1;
// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.gyro_timestamp[0];
}
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
update_vect[1] = 1;
// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
}
hrt_abstime vel_t = 0;
bool vel_valid = false;
if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
vel(0) = global_pos.vel_n;
vel(1) = global_pos.vel_e;
vel(2) = global_pos.vel_d;
}
}
if (vel_valid) {
/* velocity is valid */
if (vel_t != 0) {
/* velocity updated */
if (last_vel_t != 0 && vel_t != last_vel_t) {
float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
/* calculate acceleration in body frame */
acc = R.transposed() * ((vel - vel_prev) / vel_dt);
}
last_vel_t = vel_t;
vel_prev = vel;
}
} else {
/* velocity is valid, reset acceleration */
acc.zero();
vel_prev.zero();
last_vel_t = 0;
}
z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
/* check that the mag vector is > 0 */
fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
update_vect[2] = 1;
// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
}
bool vision_updated = false;
orb_check(vision_sub, &vision_updated);
bool mocap_updated = false;
orb_check(mocap_sub, &mocap_updated);
if (vision_updated) {
orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
}
if (mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
}
if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) {
math::Quaternion q(mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
z_k[6] = vn(0);
z_k[7] = vn(1);
z_k[8] = vn(2);
}else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
math::Quaternion q(vision.q);
math::Matrix<3, 3> Rvis = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
z_k[6] = vn(0);
z_k[7] = vn(1);
z_k[8] = vn(2);
} else {
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.001f) {
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
}
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, mag_decl);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
/* Call the estimator */
AttitudeEKF(false, // approx_prediction
(unsigned char)ekf_params.use_moment_inertia,
update_vect,
dt,
z_k,
ekf_params.q[0], // q_rotSpeed,
ekf_params.q[1], // q_rotAcc
ekf_params.q[2], // q_acc
ekf_params.q[3], // q_mag
ekf_params.r[0], // r_gyro
ekf_params.r[1], // r_accel
ekf_params.r[2], // r_mag
ekf_params.moment_inertia_J,
x_aposteriori,
P_aposteriori,
Rot_matrix,
euler,
debugOutput);
/* swap values for next iteration, check for fatal inputs */
if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) {
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 30000) {
warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data));
}
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
att.rollacc = x_aposteriori[3];
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
R = R_decl * R_body;
math::Quaternion q;
q.from_dcm(R);
/* copy rotation matrix */
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
memcpy(&att.q[0],&q.data[0],sizeof(att.q));
att.R_valid = true;
if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("ERR: NaN estimate!");
}
perf_end(ekf_loop_perf);
}
}
}
loopcounter++;
}
thread_running = false;
return 0;
}
static DF1(xn1){RZ( w); R vn(xd(0L,w,self));}
