/**
* @brief Fix the rotation of the IR dots.
*
* @param dot An array of 4 ir_dot_t objects.
* @param ang The roll angle to correct by (-180, 180)
*
* If there is roll then the dots are rotated
* around the origin and give a false cursor
* position. Correct for the roll.
*
* If the accelerometer is off then obviously
* this will not do anything and the cursor
* position may be inaccurate.
*/
static void fix_rotated_ir_dots(struct ir_dot_t* dot, float ang) {
float s, c;
int x, y;
int i;
if (!ang) {
for (i = 0; i < 4; ++i) {
dot[i].x = dot[i].rx;
dot[i].y = dot[i].ry;
}
return;
}
s = sin(DEGREE_TO_RAD(ang));
c = cos(DEGREE_TO_RAD(ang));
/*
* [ cos(theta) -sin(theta) ][ ir->rx ]
* [ sin(theta) cos(theta) ][ ir->ry ]
*/
for (i = 0; i < 4; ++i) {
if (!dot[i].visible)
continue;
x = dot[i].rx - (1024/2);
y = dot[i].ry - (768/2);
dot[i].x = (c * x) + (-s * y);
dot[i].y = (s * x) + (c * y);
dot[i].x += (1024/2);
dot[i].y += (768/2);
}
}
//Attempt to correct raw x/y values for rotation
void ExtObject::FixIrRotation()
{
float angle = remote.Acceleration.Orientation.Roll;
float s, c;
int x, y;
if(!angle)
return;
if(angle != 0) {
s = sin(DEGREE_TO_RAD(angle));
c = cos(DEGREE_TO_RAD(angle));
for(int i = 0; i < 4; i++) {
wiimote_state::ir::dot* dot = &remote.IR.Dot[i];
_ASSERT(dot);
if(!dot->bVisible)
continue;
x = dot->RawX - (remote.IR.MAX_RAW_X/2);
y = dot->RawY - (remote.IR.MAX_RAW_Y/2);
dot->X = (c * x) + (-s * y);
dot->Y = (s * x) + (c * y);
dot->X += (remote.IR.MAX_RAW_X/2);
dot->Y += (remote.IR.MAX_RAW_Y/2);
}
}
}
void TreeStem::Build(TreeSegment* parentSeg, float offset, TreeGeneralParams generalParams, TreeLevelParams levelParams,
LevelContext& context)
{
static int dbNumStems[4];
dbNumStems[levelParams.level]++;
ConsolePrint(L"current numStems: (0-%d),(1-%d),(2-%d),(3-%d)\n", dbNumStems[0], dbNumStems[1], dbNumStems[2], dbNumStems[3]);
SAFE_DELETE(mFirstSeg);
mParentSeg = parentSeg;
mLevelParams = levelParams;
float parentLength = 0;
float stemOffset = 0;
if(parentSeg)
{
mPos = Vector3(0, offset, 0); // offset: 当stem是parentSeg的一个枝条时, stem在parentSeg上长出的位置
parentLength = mParentSeg->mStem->GetLength();
stemOffset = mParentSeg->mSegIndex * mParentSeg->mStem->GetSegLength() + offset;
}
else
{
_Assert(mLevelParams.level == 0); // 只有trunk不存在parentSeg
mPos = Vector3::Zero;
}
mLength = calcStemLength(generalParams, parentLength, stemOffset);
mBaseRadius = calcStemBaseRadius(generalParams, parentLength, stemOffset);
float baseLength = 0;
if(mLevelParams.level == 0)
{
baseLength = generalParams.baseSize * mLength;
}
calcStemBranchInterval(parentLength, stemOffset, baseLength, &mNumBranches, &mBranchInterval);
float parentBaseLength = 0;
if(mLevelParams.level == 1)
{
parentBaseLength = generalParams.baseSize * parentLength;
}
float downAngle = 0;
float rotateAngle = 0;
calcBranchAngle(parentLength, stemOffset, parentBaseLength, context.rotateAngle, &downAngle, &rotateAngle);
context.rotateAngle = rotateAngle;
mOrient = Quaternion(0, DEGREE_TO_RAD(rotateAngle), DEGREE_TO_RAD(downAngle));
buildSegment(generalParams, NULL, 0, 0, 0, 0, LevelContext());
}
void turtle_t::forward(const double _dist)
{
double x = _dist * cos(DEGREE_TO_RAD(dir)) + pos.x;
double y = _dist * sin(DEGREE_TO_RAD(dir)) + pos.y;
glColor4f(col.r, col.g, col.b, 1.0);
glBegin(GL_LINES);
glVertex3f(pos.x, pos.y, 0.0f);
glVertex3f(x, y, 0.0f);
glEnd();
vertex_t newPos;
newPos.x = x;
newPos.y = y;
pos = newPos;
}
Ref FreeCam::GetRef()
{
Point3D pos = m_Vehicle->MyRef.Position;
pos.x() += REAL( cos(DEGREE_TO_RAD(m_theta))*m_radius*sin(DEGREE_TO_RAD(m_phi)) );
pos.y() += REAL( sin(DEGREE_TO_RAD(m_theta))*m_radius*sin(DEGREE_TO_RAD(m_phi)) );
pos.z() += REAL( m_radius * cos(DEGREE_TO_RAD(m_phi)) );
Point3D forward = m_Vehicle->MyRef.Position - pos;
Point3D upward = Point3D(0,0,1);
Point3D right = forward ^ upward;
upward = right ^ forward;
forward.normalize();
upward.normalize();
right.normalize();
return Ref(pos,forward,upward);
}
void Box::bend(QVector3D v, float maxAngle)
{
float heightY = m_overallObjectDimensions.y();
float rad = (heightY / 2 + v.y()) / heightY * DEGREE_TO_RAD(maxAngle);
float y = v.y();
v.setY(cos(rad) * v.y() - sin(rad) * v.x());
v.setX(sin(rad) * y + cos(rad) * v.x());
glVertex3fv(&v[0]);
}
bool SLine::is_vertical_to(SLine &other_line, int epsilon)
{
int dot_prod = dot_product(other_line);
if(epsilon)
{
//calculate
double norm1 = norm();
double norm2 = other_line.norm();
double cosfi = dot_prod/norm1/norm2;
double fi = acos(cosfi);
if(std::abs(DEGREE_TO_RAD(90.0)-fi) < epsilon)
return true;
}
else
return static_cast<bool>(std::abs(dot_prod) < 0.000001);
return false;
}
bool SLine::is_parallel_to(SLine &other_line, int epsilon)
{
double dot_prod = static_cast<double>(dot_product(other_line));
if(dot_prod < 0.0000001)
return false;
double norm1 = norm();
double norm2 = other_line.norm();
double cosfi = dot_prod/norm1/norm2;
double fi = acos(cosfi);
double diff = std::abs(DEGREE_TO_RAD(0.0)-fi);
if(epsilon)
{
if(diff < epsilon)
//calculate
return true;
}
else
{
return static_cast<bool>(std::abs(fi) < 0.000001);
}
return false;
}
bool InitializeCameraComponent(IActorComponent* cmp, ICMStream* stream)
{
CameraComponent* cameraCmp = (CameraComponent*)cmp;
tinyxml2::XMLElement* pData = (tinyxml2::XMLElement*)stream;
tinyxml2::XMLElement* settings = pData->FirstChildElement("Settings");
if(settings)
{
float fov, aspect, viewDir, n, f;
RETURN_IF_FAILED(tinyxml2::XML_NO_ATTRIBUTE != settings->QueryFloatAttribute("fov", &fov));
RETURN_IF_FAILED(tinyxml2::XML_NO_ATTRIBUTE != settings->QueryFloatAttribute("aspect", &aspect));
RETURN_IF_FAILED(tinyxml2::XML_NO_ATTRIBUTE != settings->QueryFloatAttribute("viewAxis", &viewDir));
RETURN_IF_FAILED(tinyxml2::XML_NO_ATTRIBUTE != settings->QueryFloatAttribute("far", &f));
RETURN_IF_FAILED(tinyxml2::XML_NO_ATTRIBUTE != settings->QueryFloatAttribute("near", &n));
//TODO: do we have to keep it?
tinyxml2::XMLElement* type = pData->FirstChildElement("Type");
if(type)
{
cameraCmp->m_type = type->GetText();
}
else
{
cameraCmp->m_type = std::string("FreeLook");
}
if(cameraCmp->m_type == std::string("FPSCharacter"))
{
cameraCmp->m_camera = std::shared_ptr<util::FPSCamera>(new util::CharacterCamera(
chimera::CmGetApp()->VGetHumanView()->VGetRenderer()->VGetWidth(),
chimera::CmGetApp()->VGetHumanView()->VGetRenderer()->VGetHeight(),
(float)n,
(float)f)
);
}
else if(cameraCmp->m_type == std::string("FPSShakeCharacter"))
{
cameraCmp->m_camera = std::shared_ptr<util::FPSCamera>(new util::CharacterHeadShake(
chimera::CmGetApp()->VGetHumanView()->VGetRenderer()->VGetWidth(),
chimera::CmGetApp()->VGetHumanView()->VGetRenderer()->VGetHeight(),
(float)n,
(float)f)
);
}
else if(cameraCmp->m_type == "FPSStatic")
{
cameraCmp->m_camera = std::shared_ptr<util::StaticCamera>(new util::StaticCamera(
chimera::CmGetApp()->VGetHumanView()->VGetRenderer()->VGetWidth(),
chimera::CmGetApp()->VGetHumanView()->VGetRenderer()->VGetHeight(),
(float)n,
(float)f)
);
}
else
{
LOG_CRITICAL_ERROR("FPS Camera is bugged"); //use FPSCharacter
}
cameraCmp->m_camera->SetFoV(DEGREE_TO_RAD(fov));
tinyxml2::XMLElement* activate = pData->FirstChildElement("Activate");
if(activate)
{
CmGetApp()->VGetHumanView()->VSetTarget(cameraCmp->VGetActor());
}
return true;
}
return false;
}
void TreeStem::buildSegment(TreeGeneralParams generalParams, TreeSegment* prevSeg, int segIndex,
float splitAngle, float divergeAngle, float rotateYAngle, LevelContext& context)
{
_Assert(segIndex < mLevelParams.curveRes);
static int dbNumSegs[4];
dbNumSegs[mLevelParams.level]++;
ConsolePrint(L"current numSegs: (0-%d),(1-%d),(2-%d),(3-%d)\n", dbNumSegs[0], dbNumSegs[1], dbNumSegs[2], dbNumSegs[3]);
int numSegs = mLevelParams.curveRes;
float segLength = mLength / numSegs;
TreeSegment* seg = New TreeSegment;
float curveDelta = 0;
if(segIndex != 0)
{
if(mLevelParams.curveBack == 0)
{
curveDelta = mLevelParams.curve / mLevelParams.curveRes;
}
else
{
if(segIndex < (mLevelParams.curveRes + 1) / 2)
curveDelta = 2 * mLevelParams.curve / mLevelParams.curveRes;
else
curveDelta = -2 * mLevelParams.curveBack / mLevelParams.curveRes;
}
curveDelta += RandomVariation(0, mLevelParams.curveV) / mLevelParams.curveRes;
}
Vector3 basePos;
if(prevSeg == NULL)
basePos = Vector3::Zero;
else
basePos = Vector3(0, segLength, 0);
seg->mPos = basePos;
seg->mOrient = Quaternion(0, DEGREE_TO_RAD(divergeAngle), DEGREE_TO_RAD(curveDelta + splitAngle));
seg->mSegIndex = segIndex;
seg->mParent = prevSeg;
seg->mStem = this;
Quaternion parentOrientWorld;
if(prevSeg)
{
prevSeg->mChildren.push_back(seg);
parentOrientWorld = prevSeg->mOrientWorld;
}
else
{
mFirstSeg = seg;
parentOrientWorld = mOrient;
}
// divergeAngle是绕世界的y轴旋转
seg->mOrientWorld = Quaternion(0, DEGREE_TO_RAD(rotateYAngle), 0) * (parentOrientWorld * seg->mOrient);
seg->mOrient = WorldRotationToLocal(parentOrientWorld.Difference(seg->mOrientWorld), parentOrientWorld);
// create seg vb ...
std::vector<float> segRadius;
{
float offset = (float)segIndex / numSegs;
float offsetDelta = 1.0f / numSegs / mLevelParams.segSegsH;
for(int i = 0; i <= mLevelParams.segSegsH; ++i)
{
segRadius.push_back(GetStemRadius(offset));
offset += offsetDelta;
}
}
bool closeTop = true;
bool closeBottom = true;
//if(segIndex == 0)
// closeBottom = true;
//if(segIndex == numSegs - 1)
// closeTop = true;
seg->mGeo = New TreeSegGeo(segLength, mLevelParams.segSegsW, mLevelParams.segSegsH, segRadius,
Quaternion(0, 0, DEGREE_TO_RAD(curveDelta + splitAngle)), closeTop, closeBottom);
seg->mGeo->CalculateNormals();
seg->mGeo->BuildGeometry(XYZ_N);
// branches
if(seg->mStem->GetNumBranches() != 0 && mLevelParams.level < generalParams.levels - 1)
{
float baseOffset = 0;
if(mLevelParams.level == 0)
{
float baseLength = generalParams.baseSize * mLength;
baseOffset = baseLength - segIndex * segLength;
}
float offset = 0;
if(baseOffset <= 0)
{
offset = mBranchInterval - context.branchDistError;
}
else if(baseOffset < segLength)
{
offset = baseOffset + mBranchInterval;
}
else
{
offset = segLength + mBranchInterval;
}
while(offset < segLength - 0.0001f)
{
TreeStem* branch = New TreeStem(mTree, mLevelParams.level + 1);
branch->Build(seg, offset, generalParams, mTree->GetLevelParams(mLevelParams.level + 1), context);
seg->mBranches.push_back(branch);
offset += mBranchInterval;
}
context.branchDistError = segLength - (offset - mBranchInterval);
}
// splits
if(segIndex == numSegs - 1)
return;
int numSplits = 0;
if(mLevelParams.level == 0 && segIndex == 0)
{
numSplits = generalParams.baseSplits;
}
else
{
numSplits = (int)(mLevelParams.segSplits + context.splitError);
context.splitError = mLevelParams.segSplits + context.splitError - numSplits;
}
if(numSplits < 1) // no splits
{
buildSegment(generalParams, seg, segIndex + 1, 0, 0, 0, context);
}
else
{
Vector3 vecUp = Vector3(0, 1, 0) * seg->mOrientWorld;
float declination = VectorAngle(Vector3(0, 1, 0), vecUp);
float splitAngleFactor = 1.0f; // 调整此算法来使splitAngle随枝干的水平角度加成变化
splitAngleFactor = fabs(declination - PI/2) / (PI/2);
float childSplitAngle = max(0, splitAngleFactor * RandomVariation(mLevelParams.splitAngle, mLevelParams.splitAngleV));
for(int i = 0; i <= numSplits; ++i)
{
float childDivergeAngle = 0;
float childRotateYAngle = 0;
childDivergeAngle = ((float)i / (numSplits + 1)) * 360;
if(mLevelParams.level == 0 && segIndex == 0)
{
childRotateYAngle = 0;
}
else
{
float rf = RandomFloat(0, 1.0f);
int s = sign(RandomFloat(-1.0f, 1.0f));
childRotateYAngle = s * (20 + 0.75f * (30 + fabs(declination - 90)) * pow(rf, 2));
}
buildSegment(generalParams, seg, segIndex + 1, childSplitAngle, childDivergeAngle, childRotateYAngle, context);
}
}
}
void turtle_t::backward_move(const double _dist)
{
set_pos (_dist * cos(DEGREE_TO_RAD(dir + 180)) + pos.x, _dist * sin(DEGREE_TO_RAD(dir + 180)) + pos.y);
}
void Box::twist(QVector3D v, float maxAngle)
{
float rad = - (m_overallObjectDimensions.y() / 2 + v.y())/m_overallObjectDimensions.y() * DEGREE_TO_RAD(maxAngle);
float x = (cos(rad) * v.x()) - (sin(rad) * v.z());
float y = (sin(rad) * v.x()) + (cos(rad) * v.z());
v.setX(x);
v.setZ(y);
v.setY(v.y());
glVertex3fv(&v[0]);
}
