void cEqAppui_PProjInc_M2CPolyn2::ComputeVal()
{
double tmp0_ = mCompCoord[9];
double tmp1_ = mCompCoord[10];
double tmp2_ = cos(tmp1_);
double tmp3_ = mCompCoord[15];
double tmp4_ = mCompCoord[16];
double tmp5_ = mCompCoord[17];
double tmp6_ = sin(tmp0_);
double tmp7_ = cos(tmp0_);
double tmp8_ = sin(tmp1_);
double tmp9_ = mCompCoord[11];
double tmp10_ = mLocProjI_x * tmp3_;
double tmp11_ = mLocProjP0_x + tmp10_;
double tmp12_ = mLocProjJ_x * tmp4_;
double tmp13_ = tmp11_ + tmp12_;
double tmp14_ = mLocProjK_x * tmp5_;
double tmp15_ = tmp13_ + tmp14_;
double tmp16_ = mCompCoord[12];
double tmp17_ = (tmp15_) - tmp16_;
double tmp18_ = sin(tmp9_);
double tmp19_ = -(tmp18_);
double tmp20_ = -(tmp8_);
double tmp21_ = cos(tmp9_);
double tmp22_ = mLocProjI_y * tmp3_;
double tmp23_ = mLocProjP0_y + tmp22_;
double tmp24_ = mLocProjJ_y * tmp4_;
double tmp25_ = tmp23_ + tmp24_;
double tmp26_ = mLocProjK_y * tmp5_;
double tmp27_ = tmp25_ + tmp26_;
double tmp28_ = mCompCoord[13];
double tmp29_ = (tmp27_) - tmp28_;
double tmp30_ = mLocProjI_z * tmp3_;
double tmp31_ = mLocProjP0_z + tmp30_;
double tmp32_ = mLocProjJ_z * tmp4_;
double tmp33_ = tmp31_ + tmp32_;
double tmp34_ = mLocProjK_z * tmp5_;
double tmp35_ = tmp33_ + tmp34_;
double tmp36_ = mCompCoord[14];
double tmp37_ = (tmp35_) - tmp36_;
double tmp38_ = -(tmp6_);
double tmp39_ = tmp7_ * tmp20_;
double tmp40_ = tmp6_ * tmp20_;
double tmp41_ = mCompCoord[0];
double tmp42_ = tmp38_ * tmp19_;
double tmp43_ = tmp39_ * tmp21_;
double tmp44_ = tmp42_ + tmp43_;
double tmp45_ = (tmp44_) * (tmp17_);
double tmp46_ = tmp7_ * tmp19_;
double tmp47_ = tmp40_ * tmp21_;
double tmp48_ = tmp46_ + tmp47_;
double tmp49_ = (tmp48_) * (tmp29_);
double tmp50_ = tmp45_ + tmp49_;
double tmp51_ = tmp2_ * tmp21_;
double tmp52_ = tmp51_ * (tmp37_);
double tmp53_ = tmp50_ + tmp52_;
double tmp54_ = tmp41_ / (tmp53_);
double tmp55_ = tmp7_ * tmp2_;
double tmp56_ = tmp55_ * (tmp17_);
double tmp57_ = tmp6_ * tmp2_;
double tmp58_ = tmp57_ * (tmp29_);
double tmp59_ = tmp56_ + tmp58_;
double tmp60_ = tmp8_ * (tmp37_);
double tmp61_ = tmp59_ + tmp60_;
double tmp62_ = (tmp61_) * (tmp54_);
double tmp63_ = mCompCoord[1];
double tmp64_ = tmp62_ + tmp63_;
double tmp65_ = (tmp64_) - mLocPolyn2_State_1_0;
double tmp66_ = (tmp65_) / mLocPolyn2_State_0_0;
double tmp67_ = tmp38_ * tmp21_;
double tmp68_ = tmp39_ * tmp18_;
double tmp69_ = tmp67_ + tmp68_;
double tmp70_ = (tmp69_) * (tmp17_);
double tmp71_ = tmp7_ * tmp21_;
double tmp72_ = tmp40_ * tmp18_;
double tmp73_ = tmp71_ + tmp72_;
double tmp74_ = (tmp73_) * (tmp29_);
double tmp75_ = tmp70_ + tmp74_;
double tmp76_ = tmp2_ * tmp18_;
double tmp77_ = tmp76_ * (tmp37_);
double tmp78_ = tmp75_ + tmp77_;
double tmp79_ = (tmp78_) * (tmp54_);
double tmp80_ = mCompCoord[2];
double tmp81_ = tmp79_ + tmp80_;
double tmp82_ = (tmp81_) - mLocPolyn2_State_2_0;
double tmp83_ = (tmp82_) / mLocPolyn2_State_0_0;
double tmp84_ = mCompCoord[3];
double tmp85_ = mCompCoord[4];
double tmp86_ = mCompCoord[5];
double tmp87_ = (tmp66_) * (tmp83_);
double tmp88_ = mCompCoord[6];
double tmp89_ = (tmp83_) * (tmp83_);
double tmp90_ = (tmp66_) * (tmp66_);
mVal[0] = ((mLocPolyn2_State_1_0 + (((1 + tmp84_) * (tmp66_) + tmp85_ * (tmp83_)) - tmp86_ * 2 * tmp90_ + tmp88_ * tmp87_ + mCompCoord[7] * tmp89_) * mLocPolyn2_State_0_0) - mLocXIm) * mLocScNorm;
mVal[1] = ((mLocPolyn2_State_2_0 + (((1 - tmp84_) * (tmp83_) + tmp85_ * (tmp66_) + tmp86_ * tmp87_) - tmp88_ * 2 * tmp89_ + mCompCoord[8] * tmp90_) * mLocPolyn2_State_0_0) - mLocYIm) * mLocScNorm;
}
double r8_gamma ( double x )
/******************************************************************************/
/*
Purpose:
R8_GAMMA evaluates Gamma(X) for a real argument.
Discussion:
The C math library includes the GAMMA ( X ) function which should generally
be used instead of this function.
This routine calculates the gamma function for a real argument X.
Computation is based on an algorithm outlined in reference 1.
The program uses rational functions that approximate the gamma
function to at least 20 significant decimal digits. Coefficients
for the approximation over the interval (1,2) are unpublished.
Those for the approximation for 12 <= X are from reference 2.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
11 January 2010
Author:
Original FORTRAN77 version by William Cody, Laura Stoltz.
C version by John Burkardt.
Reference:
William Cody,
An Overview of Software Development for Special Functions,
in Numerical Analysis Dundee, 1975,
edited by GA Watson,
Lecture Notes in Mathematics 506,
Springer, 1976.
John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
Charles Mesztenyi, John Rice, Henry Thatcher,
Christoph Witzgall,
Computer Approximations,
Wiley, 1968,
LC: QA297.C64.
Parameters:
Input, double X, the argument of the function.
Output, double R8_GAMMA, the value of the function.
*/
{
double c[7] = {
-1.910444077728E-03,
8.4171387781295E-04,
-5.952379913043012E-04,
7.93650793500350248E-04,
-2.777777777777681622553E-03,
8.333333333333333331554247E-02,
5.7083835261E-03 };
double eps = 2.22E-16;
double fact;
int i;
int n;
double p[8] = {
-1.71618513886549492533811E+00,
2.47656508055759199108314E+01,
-3.79804256470945635097577E+02,
6.29331155312818442661052E+02,
8.66966202790413211295064E+02,
-3.14512729688483675254357E+04,
-3.61444134186911729807069E+04,
6.64561438202405440627855E+04 };
int parity;
const double pi = 3.1415926535897932384626434;
double q[8] = {
-3.08402300119738975254353E+01,
3.15350626979604161529144E+02,
-1.01515636749021914166146E+03,
-3.10777167157231109440444E+03,
2.25381184209801510330112E+04,
4.75584627752788110767815E+03,
-1.34659959864969306392456E+05,
-1.15132259675553483497211E+05 };
double res;
const double sqrtpi = 0.9189385332046727417803297;
double sum;
double value;
double xbig = 171.624;
double xden;
double xinf = 1.79E+308;
double xminin = 2.23E-308;
double xnum;
double y;
double y1;
double ysq;
double z;
parity = 0;
fact = 1.0;
n = 0;
y = x;
/*
Argument is negative.
*/
if ( y <= 0.0 )
{
y = - x;
y1 = ( double ) ( int ) ( y );
res = y - y1;
if ( res != 0.0 )
{
if ( y1 != ( double ) ( int ) ( y1 * 0.5 ) * 2.0 )
{
parity = 1;
}
fact = - pi / sin ( pi * res );
y = y + 1.0;
}
else
{
res = xinf;
value = res;
return value;
}
}
/*
Argument is positive.
*/
if ( y < eps )
{
/*
Argument < EPS.
*/
if ( xminin <= y )
{
res = 1.0 / y;
}
else
{
res = xinf;
value = res;
return value;
}
}
else if ( y < 12.0 )
{
y1 = y;
/*
0.0 < argument < 1.0.
*/
if ( y < 1.0 )
{
z = y;
y = y + 1.0;
}
/*
1.0 < argument < 12.0.
Reduce argument if necessary.
*/
else
{
n = ( int ) ( y ) - 1;
y = y - ( double ) ( n );
z = y - 1.0;
}
/*
Evaluate approximation for 1.0 < argument < 2.0.
*/
xnum = 0.0;
xden = 1.0;
for ( i = 0; i < 8; i++ )
{
xnum = ( xnum + p[i] ) * z;
xden = xden * z + q[i];
}
res = xnum / xden + 1.0;
/*
Adjust result for case 0.0 < argument < 1.0.
*/
if ( y1 < y )
{
res = res / y1;
}
/*
Adjust result for case 2.0 < argument < 12.0.
*/
else if ( y < y1 )
{
for ( i = 1; i <= n; i++ )
{
res = res * y;
y = y + 1.0;
}
}
}
else
{
/*
Evaluate for 12.0 <= argument.
*/
if ( y <= xbig )
{
ysq = y * y;
sum = c[6];
for ( i = 0; i < 6; i++ )
{
sum = sum / ysq + c[i];
}
sum = sum / y - y + sqrtpi;
sum = sum + ( y - 0.5 ) * log ( y );
res = exp ( sum );
}
else
{
res = xinf;
value = res;
return value;
}
}
/*
Final adjustments and return.
*/
if ( parity )
{
res = - res;
}
if ( fact != 1.0 )
{
res = fact / res;
}
value = res;
return value;
}
//--------------------------------------------------------------
void AppCore::setup(const int numOutChannels, const int numInChannels,
const int sampleRate, const int ticksPerBuffer) {
ofSetFrameRate(30);
ofSetVerticalSync(true);
//ofSetLogLevel(OF_LOG_VERBOSE);
// double check where we are ...
cout << ofFilePath::getCurrentWorkingDirectory() << endl;
//----------------------------------- PD START-------------------------------------------
if(!pd.init(numOutChannels, numInChannels, sampleRate, ticksPerBuffer)) {
OF_EXIT_APP(1);
}
Externals::setup();
midiChan = 1; // midi channels are 1-16
// subscribe to receive source names
pd.subscribe("toOf");
pd.subscribe("env");
pd.addReceiver(*this); // automatically receives from all subscribed sources
pd.ignore(*this, "env"); // don't receive from "env"
pd.addMidiReceiver(*this); // automatically receives from all channels
pd.addToSearchPath("pd/abs");
pd.start();
//Patch patch = pd.openPatch("pd/somename.pd");
//cout << patch << endl;
//----------------------------------- PD END-------------------------------------------
//----------------------------------- KINECT START -------------------------------------------
kinect.listDevices();
kinect.init();
kinect.open("A00365917784047A");
kinect.setCameraTiltAngle(0);
grayImage.allocate(kinect.width, kinect.height);
kinect1.init();
kinect1.open("A00364A11700045A");
kinect1.setCameraTiltAngle(0);
grayImage1.allocate(kinect1.width, kinect1.height);
bothKinects.allocate(kinect.height*2, kinect.width);
combinedVideo = (unsigned char*)malloc(640 * 480 * 2 * sizeof(unsigned char*));
blobs.resize(100);
blobCenterX.resize(100);
blobCenterXmap.resize(100);
blobCenterY.resize(100);
blobCenterYmap.resize(100);
//----------------------------------- KINECT END -------------------------------------------
// Setup OSC Sender
sender.setup(HOST, PORT);
allPipes = new ofPipe*[TUBE_NUM]; // an array of pointers for the objects
persons = new ofPerson*[PERSON_NUM];
//the string is printed at the top of the app
//to give the user some feedback
message = "loading data.xml";
//we load our data file
if( XML.loadFile("data.xml") ){
message = "data.xml loaded!";
}else{
message = "unable to load data.xml check data/ folder";
}
XML.pushTag("document");
newXML.addTag("document");
newXML.pushTag("document");
for(int i=0; i<TUBE_NUM; i++){
XML.pushTag("tube",i);
float x = XML.getValue("y",0.0);
float y = XML.getValue("x",0.0);
// rotate
float angle = ofDegToRad(-20);
float rX = (x*cos(angle) - y*sin(angle));
float rY = (x*sin(angle) + y*cos(angle));
// map to the of window size
float mult = 6;
x = (rX * mult + ofGetWidth() / 2) - 50;
y = rY * mult + ofGetHeight() / 2;
float radius = XML.getValue("diameter",0.0 ) / 1.8;
float length = XML.getValue("length",0.0 );
float height= XML.getValue("height",0.0);
float frequency = 342 / ((length*2)/100);
int idNum = XML.getValue("num",0);
int element = XML.getValue("element",0 );
int open = XML.getValue("oc",0 );
allPipes[i] = new ofPipe(x,y,radius,length,height,frequency, idNum, element, open);
newXML.addTag("tube");
newXML.pushTag("tube", i);
newXML.addValue("num", idNum);
newXML.addValue("length", length);
newXML.addValue("height", height);
newXML.addValue("diameter", radius);
newXML.addValue("x", ofMap(x, 0, ofGetWidth(), 0, 1));
newXML.addValue("y", ofMap(y, 0, ofGetHeight(), 0, 1));
newXML.addValue("element", element);
newXML.addValue("oc", open);
XML.popTag();
newXML.popTag();
}
newXML.saveFile("newXML.xml");
// load the PD patches and create the people
for(int i = 0; i<PERSON_NUM; i++){
//patches[i] = pd.openPatch("pd/main.pd");
persons[i] = new ofPerson(0.0,0.0,0.0, i);
}
// load a separate patch for the mouse
//mousePatch = pd.openPatch("pd/main.pd");
mPerson = new ofPerson(0.0,0.0,0.0,0);
outputState=false;
}
// compute a point on ellipse from theta
void Ellipse(double theta, double *x, double *y)
{
*x = Rx * cos(theta) + Cx;
*y = Ry * sin(theta) + Cy;
}
void TacticsInstrument_AppTrueWindAngle::DrawForeground(wxGCDC* dc)
{
wxPoint points[4];
double data;
double val;
double value;
// The default foreground is the arrow used in most dials
wxColour cl;
GetGlobalColor(_T("DASH2"), &cl);
wxPen pen1;
pen1.SetStyle(wxSOLID);
pen1.SetColour(cl);
pen1.SetWidth(2);
dc->SetPen(pen1);
GetGlobalColor(_T("DASH1"), &cl);
wxBrush brush1;
brush1.SetStyle(wxSOLID);
brush1.SetColour(cl);
dc->SetBrush(brush1);
dc->DrawCircle(m_cx, m_cy, m_radius / 8);
/*True Wind*/
if (!wxIsNaN(m_ExtraValueTrue)){ //m_ExtraValueTrue = True Wind Angle; we have a watchdog for TWS; if TWS becomes NAN, TWA must be NAN as well
dc->SetPen(*wxTRANSPARENT_PEN);
GetGlobalColor(_T("BLUE3"), &cl);
wxBrush brush2;
brush2.SetStyle(wxSOLID);
brush2.SetColour(cl);
dc->SetBrush(brush2);
/* this is fix for a +/-180° round instrument, when m_MainValue is supplied as <0..180><L | R>
* for example TWA & AWA */
if (m_MainValueTrueUnit == _T("\u00B0L"))
data = 360 - m_MainValueTrue;
else
data = m_MainValueTrue;
// The arrow should stay inside fixed limits
if (data < m_MainValueMin) val = m_MainValueMin;
else if (data > m_MainValueMax) val = m_MainValueMax;
else val = data;
value = deg2rad((val - m_MainValueMin) * m_AngleRange / (m_MainValueMax - m_MainValueMin)) + deg2rad(m_AngleStart - ANGLE_OFFSET);
points[0].x = m_cx + (m_radius * 0.95 * cos(value - .010));
points[0].y = m_cy + (m_radius * 0.95 * sin(value - .010));
points[1].x = m_cx + (m_radius * 0.95 * cos(value + .015));
points[1].y = m_cy + (m_radius * 0.95 * sin(value + .015));
points[2].x = m_cx + (m_radius * 0.22 * cos(value + 2.8));
points[2].y = m_cy + (m_radius * 0.22 * sin(value + 2.8));
points[3].x = m_cx + (m_radius * 0.22 * cos(value - 2.8));
points[3].y = m_cy + (m_radius * 0.22 * sin(value - 2.8));
dc->DrawPolygon(4, points, 0, 0);
}
/* Apparent Wind*/
if (!wxIsNaN(m_ExtraValueApp)){ //m_ExtraValueApp=AWA; we have a watchdog for AWS; if AWS becomes NAN, AWA will also be NAN ...
dc->SetPen(*wxTRANSPARENT_PEN);
GetGlobalColor(_T("DASHN"), &cl);
wxBrush brush;
brush.SetStyle(wxSOLID);
brush.SetColour(cl);
dc->SetBrush(brush);
/* this is fix for a +/-180° round instrument, when m_MainValue is supplied as <0..180><L | R>
* for example TWA & AWA */
if (m_MainValueAppUnit == _T("\u00B0L"))
data = 360 - m_MainValueApp;
else
data = m_MainValueApp;
// The arrow should stay inside fixed limits
if (data < m_MainValueMin) val = m_MainValueMin;
else if (data > m_MainValueMax) val = m_MainValueMax;
else val = data;
value = deg2rad((val - m_MainValueMin) * m_AngleRange / (m_MainValueMax - m_MainValueMin)) + deg2rad(m_AngleStart - ANGLE_OFFSET);
points[0].x = m_cx + (m_radius * 0.95 * cos(value - .010));
points[0].y = m_cy + (m_radius * 0.95 * sin(value - .010));
points[1].x = m_cx + (m_radius * 0.95 * cos(value + .015));
points[1].y = m_cy + (m_radius * 0.95 * sin(value + .015));
points[2].x = m_cx + (m_radius * 0.22 * cos(value + 2.8));
points[2].y = m_cy + (m_radius * 0.22 * sin(value + 2.8));
points[3].x = m_cx + (m_radius * 0.22 * cos(value - 2.8));
points[3].y = m_cy + (m_radius * 0.22 * sin(value - 2.8));
dc->DrawPolygon(4, points, 0, 0);
}
}
bool CCP4File::readHeader()
{
// first read the complete 1024 bytes of header information
char header[1024];
std::fstream::read(header, 1024);
if (gcount() != 1024)
{
Log.error() << "CCP4File::readHeader(): File does not contain a proper CCP4 header. Aborting read." << std::endl;
return false;
}
// Currently only data_mode=2 is allowed, which stores density values as 4-byte float values
Index data_mode = readBinValueasInt_(header, 3);
if (data_mode != 2)
{
// try to change endianness
swap_bytes_= true;
data_mode = readBinValueasInt_(header, 3);
if (data_mode != 2)
{
Log.error() << "CCP4File::readHeader(): Corrupt CCP4 header: data mode not supported, only 32-bit float supported" << std::endl;
return false;
}
}
//check if file claims to have symmetry reocrds stored
Size size_of_symops = readBinValueasInt_(header, 23);
if (size_of_symops != 0)
{
offset_symops_ = size_of_symops;
}
// check internal ordering of coordinate axis
col_axis_ = readBinValueasInt_(header, 16)-1;
row_axis_ = readBinValueasInt_(header, 17)-1;
sec_axis_ = readBinValueasInt_(header, 18)-1;
extent_.x = (float)readBinValueasInt_(header, 0+col_axis_);
extent_.y = (float)readBinValueasInt_(header, 0+row_axis_);
extent_.z = (float)readBinValueasInt_(header, 0+sec_axis_);
start_.x = (float)readBinValueasInt_(header, 4+col_axis_);
start_.y = (float)readBinValueasInt_(header, 4+row_axis_);
start_.z = (float)readBinValueasInt_(header, 4+sec_axis_);
sampling_rate_.x = (float)readBinValueasInt_(header, 7);
sampling_rate_.y = (float)readBinValueasInt_(header, 8);
sampling_rate_.z = (float)readBinValueasInt_(header, 9);
cell_dimension_.x = readBinValueasFloat_(header, 10);
cell_dimension_.y = readBinValueasFloat_(header, 11);
cell_dimension_.z = readBinValueasFloat_(header, 12);
// Angle values of 0 don't make sense, set the Angles to 90 deg
if ( readBinValueasFloat_(header, 13) == 0
|| readBinValueasFloat_(header, 14) == 0
|| readBinValueasFloat_(header, 15) == 0)
{
alpha_ = Angle(90.,false);
beta_ = Angle(90.,false);
gamma_ = Angle(90.,false);
}
else
{
alpha_ = Angle(readBinValueasFloat_(header, 13),false);
beta_ = Angle(readBinValueasFloat_(header, 14),false);
gamma_ = Angle(readBinValueasFloat_(header, 15),false);
}
mean_density_ = readBinValueasFloat_(header, 21);
space_group_ = readBinValueasInt_(header, 22);
deviation_sigma_ = readBinValueasFloat_(header, 54);
Log.info() << "Mean from file: " << mean_density_ << std::endl;
Log.info() << "Sigma from file: " << deviation_sigma_ << std::endl;
// convert from grid space to cartesian coordinates
Vector3 scaled_axes(cell_dimension_.x/sampling_rate_.x,
cell_dimension_.y/sampling_rate_.y,
cell_dimension_.z/sampling_rate_.z);
Vector3 x_tmp(scaled_axes.x, 0., 0.);
Vector3 y_tmp(cos(gamma_.toRadian()), sin(gamma_.toRadian()), 0.);
y_tmp *= scaled_axes.y;
Vector3 z_tmp( cos(beta_.toRadian()),
(cos(alpha_.toRadian()) - cos(beta_.toRadian())*cos(gamma_.toRadian())) / sin(gamma_.toRadian()),
0.);
z_tmp.z = sqrt(1.0 - z_tmp.x*z_tmp.x - z_tmp.y*z_tmp.y);
z_tmp *= scaled_axes.z;
origin_.x = x_tmp.x * start_.x + y_tmp.x * start_.y + z_tmp.x * start_.z;
origin_.y = y_tmp.y * start_.y + z_tmp.y * start_.z;
origin_.z = z_tmp.z * start_.z;
xaxis_.x = x_tmp.x * (extent_.x - 1);
xaxis_.y = 0.;
xaxis_.z = 0.;
yaxis_.x = y_tmp.x * (extent_.y - 1);
yaxis_.y = y_tmp.y * (extent_.y - 1);
yaxis_.z = 0.;
zaxis_.x = z_tmp.x * (extent_.z - 1);
zaxis_.y = z_tmp.y * (extent_.z - 1);
zaxis_.z = z_tmp.z * (extent_.z - 1);
// that's it. we're done
return true;
}
void playerCamera(Camera *cam, Data *data, Player *p) {
float dest[3];
float tdest[3];
float phi, chi, r;
/* first, process all movement commands */
/* that means, check for mouse input mainly */
/* dt hack: the time since the last frame is not necessarily the game
time, since the game maybe hasn't started yet, or was paused */
static Uint32 last=0;
Uint32 dt;
if(game2->time.dt == 0) {
dt = SDL_GetTicks() - last;
last = SDL_GetTicks();
} else {
dt = game2->time.dt;
}
if(cam->type->freedom[CAM_FREE_R]) {
if(game2->input.mouse1 == 1)
cam->movement[CAM_R] += (cam->movement[CAM_R]-CLAMP_R_MIN+1) * dt / 300.0;
if(game2->input.mouse2 == 1)
cam->movement[CAM_R] -= (cam->movement[CAM_R]-CLAMP_R_MIN+1) * dt / 300.0;
writeCamDefaults(cam, CAM_R);
}
if(cam->type->freedom[CAM_FREE_PHI]) {
cam->movement[CAM_PHI] += - game2->input.mousex * MOUSE_CX;
writeCamDefaults(cam, CAM_CHI);
}
if(cam->type->freedom[CAM_FREE_CHI]) {
cam->movement[CAM_CHI] += game2->input.mousey * MOUSE_CY;
writeCamDefaults(cam, CAM_PHI);
}
/* done with mouse movement, now clamp the camera to legel values */
clampCam(cam);
phi = cam->movement[CAM_PHI];
chi = cam->movement[CAM_CHI];
r = cam->movement[CAM_R];
/* if the cam is coupled to player movement, change the phi accordingly */
if(cam->type->coupled) {
int time;
time = game2->time.current - p->data->turn_time;
if(time < TURN_LENGTH) {
int dir, ldir;
dir = p->data->dir;
ldir = p->data->last_dir;
if(dir == 1 && ldir == 2)
dir = 4;
if(dir == 2 && ldir == 1)
ldir = 4;
phi += ((TURN_LENGTH - time) * camAngles[ldir] +
time * camAngles[dir]) / TURN_LENGTH;
}
else
phi += camAngles[p->data->dir];
}
/* position the camera */
dest[0] = data->posx + r * cos(phi) * sin(chi);
dest[1] = data->posy + r * sin(phi) * sin(chi);
dest[2] = r * cos(chi);
/* ok, now let's calculate the new camera destination coordinates */
/* also, perform some camera dependant movement */
switch(cam->type->type) {
case CAM_TYPE_CIRCLING: /* Andi-cam */
cam->movement[CAM_PHI] += CAM_SPEED * game2->time.dt;
tdest[0] = data->posx;
tdest[1] = data->posy;
tdest[2] = B_HEIGHT;
break;
case CAM_TYPE_FOLLOW: /* Mike-cam */
tdest[0] = data->posx;
tdest[1] = data->posy;
tdest[2] = B_HEIGHT;
break;
case CAM_TYPE_COCKPIT: /* 1st person */
tdest[0] = data->posx + 4.0 * dirsX[ p->data->dir ] + 2.0 * cos(phi);
tdest[1] = data->posy + 4.0 * dirsY[ p->data->dir ] + 2.0 * sin(phi);
tdest[2] = CAM_COCKPIT_Z;
dest[0] = data->posx + 4.0 * dirsX[ p->data->dir ] + 0.1 * cos(phi);
dest[1] = data->posy + 4.0 * dirsY[ p->data->dir ] + 0.1 * sin(phi);
dest[2] = CAM_COCKPIT_Z + 0.1;
break;
case CAM_TYPE_MOUSE: /* mouse camera */
tdest[0] = data->posx;
tdest[1] = data->posy;
tdest[2] = B_HEIGHT;
break;
}
memcpy(cam->cam, dest, sizeof(cam->cam));
memcpy(cam->target, tdest, sizeof(cam->target));
}
void LegFinder::laserCallback(std::vector<float>& laser_r, std::vector<float>& laser_t)
{
if(laser_r.size() != laser_t.size())
{
std::cout << "LegFinder.->Something went wrong while getting laser readings :'(" << std::endl;
return;
}
this->rec.clear();
std::vector<float> laser_x;
std::vector<float> laser_y;
this->laserFilter_Mean(laser_r);
for(size_t i=0; i < laser_r.size(); i++)
{
laser_x.push_back(laser_r[i] * cos(laser_t[i]));
laser_y.push_back(laser_r[i] * sin(laser_t[i]));
}
std::vector<float> laser_flank;
std::vector<float> flank_id0;
std::vector<float> flank_id1;
std::vector<bool> flank_id2;
int ant2 = 0;
for(int i= 1; i < laser_r.size(); i++)
{
pcl::PointXYZ cua;
float sumax, sumay, px, py, m1, m2, ang;
int ant = ant2;
if(fabs(laser_r[i] - laser_r[i-1]) > FLANK_THRESHOLD)
{
if((pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) > PIERNA_DELGADA &&
(pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) < PIERNA_GRUESA)
{
if(esPierna(laser_x[ant], laser_y[ant], laser_x[i-1], laser_y[i-1]) ||
esPierna(laser_x[ant+1], laser_y[ant+1], laser_x[i-2], laser_y[i-2]))
{
sumax = 0;
sumay = 0;
for(int j= ant; j < i; j++)
{
sumax += laser_x[j];
sumay += laser_y[j];
}
flank_id0.push_back(sumax / (float)(i - ant));
flank_id1.push_back(sumay / (float)(i - ant));
flank_id2.push_back(false);
}
}
else
{
if((pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) > DOS_PIERNAS_DELGADAS &&
(pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) < DOS_PIERNAS_GRUESAS)
{
if(esPierna(laser_x[ant], laser_y[ant], laser_x[i-1], laser_y[i-1]) ||
esPierna(laser_x[ant+1], laser_y[ant+1], laser_x[i-2], laser_y[i-2]))
{
sumax = 0;
sumay = 0;
for(int j= ant; j < i; j++)
{
sumax += laser_x[j];
sumay += laser_y[j];
}
cua.x = sumax / (float)(i - ant);
cua.y = sumay / (float)(i - ant);
cua.z = 2;
this->rec.push_back(cua);
}
}
}
ant2 = i;
}
else
{
laser_flank.push_back(0);
}
}
for(int i=0; i < (int)(flank_id1.size())-2; i++)
{
for(int j=1; j < 3; j++)
{
pcl::PointXYZ cua;
float px, py;
if((pow(flank_id0[i] - flank_id0[i+j], 2) + pow(flank_id1[i] - flank_id1[i+j], 2)) > DOS_PIERNAS_CERCAS &&
(pow(flank_id0[i] - flank_id0[i+j], 2) + pow(flank_id1[i] - flank_id1[i+j], 2)) < DOS_PIERNAS_LEJOS)
{
px = (flank_id0[i] + flank_id0[i + j])/2;
py = (flank_id1[i] + flank_id1[i + j])/2;
if((px*px + py*py) < HORIZON_THRESHOLD)
{
cua.x = px;
cua.y = py;
cua.z = 2;
this->rec.push_back(cua);
flank_id2[i] = true;
flank_id2[i+j] = true;
}
}
}
}
if(flank_id1.size() > 1)
{
pcl::PointXYZ cua;
float px, py;
if((pow(flank_id0[flank_id1.size()-2] - flank_id0[flank_id1.size()-1], 2) +
pow(flank_id1[flank_id1.size()-2] - flank_id1[flank_id1.size()-1], 2)) > DOS_PIERNAS_CERCAS &&
(pow(flank_id0[flank_id1.size()-2] - flank_id0[flank_id1.size()-1], 2) +
pow(flank_id1[flank_id1.size()-2] - flank_id1[flank_id1.size()-1], 2)) < DOS_PIERNAS_LEJOS)
{
px = (flank_id0[flank_id1.size()-2] + flank_id0[flank_id1.size()-1])/2.0;
py = (flank_id1[flank_id1.size()-2] + flank_id1[flank_id1.size()-1])/2.0;
if((px*px + py*py) < HORIZON_THRESHOLD)
{
cua.x = px;
cua.y = py;
cua.z = 2;
this->rec.push_back(cua);
flank_id2[flank_id1.size() - 2] = true;
flank_id2[flank_id1.size() - 1] = true;
}
}
}
for(int i=0; i < flank_id1.size(); i++)
{
if(!flank_id2[i])
{
pcl::PointXYZ cua;
cua.x = flank_id0[i];
cua.y = flank_id1[i];
cua.z = 1;
this->rec.push_back(cua);
}
}
}
vec3 camera::getVector()
{
// std::cout << camYaw << " " << camPitch << std::endl;
return (vec3(-cos(camPitch*3.14/180.0)*sin(camYaw*3.14/180.0),sin(camPitch*3.14/180.0),-cos(camPitch*3.14/180.0)*cos(camYaw*3.14/180.0)));
}
TextRenderable::TextRenderable(const TextSymbol* symbol, const TextObject* text_object, const MapColor* color, double anchor_x, double anchor_y, bool framing_line)
: Renderable(color)
{
const QFont& font(symbol->getQFont());
const QFontMetricsF& metrics(symbol->getFontMetrics());
this->anchor_x = anchor_x;
this->anchor_y = anchor_y;
this->rotation = text_object->getRotation();
scale_factor = symbol->getFontSize() / TextSymbol::internal_point_size;
this->framing_line = framing_line;
framing_line_width = framing_line ? (2 * 0.001 * symbol->getFramingLineHalfWidth() / scale_factor) : 0;
path.setFillRule(Qt::WindingFill); // Otherwise, when text and an underline intersect, holes appear
int num_lines = text_object->getNumLines();
for (int i=0; i < num_lines; i++)
{
const TextObjectLineInfo* line_info = text_object->getLineInfo(i);
double line_y = line_info->line_y;
double underline_x0 = 0.0;
double underline_y0 = line_info->line_y + metrics.underlinePos();
double underline_y1 = underline_y0 + metrics.lineWidth();
int num_parts = line_info->part_infos.size();
for (int j=0; j < num_parts; j++)
{
const TextObjectPartInfo& part(line_info->part_infos.at(j));
if (font.underline())
{
if (j > 0)
{
// draw underline for gap between parts as rectangle
// TODO: watch out for inconsistency between text and gap underline
path.moveTo(underline_x0, underline_y0);
path.lineTo(part.part_x, underline_y0);
path.lineTo(part.part_x, underline_y1);
path.lineTo(underline_x0, underline_y1);
path.closeSubpath();
}
underline_x0 = part.part_x;
}
path.addText(part.part_x, line_y, font, part.part_text);
}
}
extent = path.controlPointRect();
extent = QRectF(scale_factor * (extent.left() - 0.5f * framing_line_width),
scale_factor * (extent.top() - 0.5f * framing_line_width),
scale_factor * (extent.width() + framing_line_width),
scale_factor * (extent.height() + framing_line_width));
if (rotation != 0)
{
float rcos = cos(-rotation);
float rsin = sin(-rotation);
std::vector<QPointF> extent_corners;
extent_corners.push_back(extent.topLeft());
extent_corners.push_back(extent.topRight());
extent_corners.push_back(extent.bottomRight());
extent_corners.push_back(extent.bottomLeft());
for (int i = 0; i < 4; ++i)
{
auto x = extent_corners[i].x() * rcos - extent_corners[i].y() * rsin;
auto y = extent_corners[i].y() * rcos + extent_corners[i].x() * rsin;
if (i == 0)
extent = QRectF(x, y, 0, 0);
else
rectInclude(extent, QPointF(x, y));
}
}
extent = QRectF(extent.left() + anchor_x, extent.top() + anchor_y, extent.width(), extent.height());
Q_ASSERT(extent.right() < 999999); // assert if bogus values are returned
}
/*
김연아 선수의 모션을 취하는 함수
*/
void Show()
{
sndPlaySound(TEXT("C:\\sample4.wav"), SND_ASYNC | SND_NOSTOP);
glLoadIdentity(); //CTM 초기화
/*
로봇의 기본적인 관절의 움직임 범위를 제한하는 곳
*/
L_Arm_x = (-40) + sin(time2) * 60;//왼쪽 어깨의 각도시작은 -40상태에서 sin()함수를 사용하여 주기적인 움직임 설정
R_Arm_x = (-80) - L_Arm_x; //우측 어깨의 각도시작은 -80상태에서 왼쪽어깨 움직임의 반대로 설정
R_Arm_y = -abs(cos(time2) * 10); //우측팔뚝 각도조절(팔을 뻗는 움직임표현을위하여 어깨의 sin()함수와 반대인 cos()함수 사용)
L_Arm_y = -abs(-cos(time2) * 10); //좌측팔뚝 각도조절(팔을 뻗는 움직임표현을위하여 어깨의 sin()함수와 반대인 cos()함수 사용)
R_Leg_y = abs(-sin(time) * 30 - 30); //우측종아리 각도조절(abs절대값을 줌으로써 종아리가 앞으로 꺾이지 않는 한계점을 설정)
L_Leg_y = abs(sin(time) * 30 - 30); //좌측종아리 각도조절(abs절대값을 줌으로써 종아리가 앞으로 꺾이지 않는 한계점을 설정)
R_Leg_x = sin(time) * 60; //우측다리는 60도 각도까지 움직이되 sin()함수를 사용하여 주기적인 움직임 설정
L_Leg_x = -R_Leg_x; //좌측다리는 우측다리반대로 60도 각도까지 움직이되 sin()함수를 사용하여 주기적인 움직임 설정
////////////////display////////////////
cyl = gluNewQuadric(); //실린더 객체 생성
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //초기화
glMatrixMode(GL_MODELVIEW); //모드 설정
DrawGround(); //지면 호출
glLoadIdentity(); //CTM 초기화
glRotatef(-230.0, 0, 1, 0); //y축기준으로 회전
/*
로봇이 피겨동작시 몸이 틀어지는 것을 표현
*/
glRotatef(sin(time) * 7, 0, 0, 1); //z축기준으로 7도 까지 각도틀어짐 (sin()함수를 사용하여 주기적인 움직임 설정)
glRotatef(sin(time) * 7, 0, 1, 0); //y축으로 7도 까지 각도틀어짐 (sin()함수를 사용하여 주기적인 움직임 설정)
//로봇 몸체 각도 조절
glTranslatef(0.0, 0.18, 0.0); //y축으로 이동
glRotatef(80, 1, 0, 0); //x축 기준으로 회전
glTranslatef(0.0, 0.5, 0.0);//최초 위치
glPushMatrix(); // 처음 저장 위치
DrawBody(0, 0, 0, 0); // 몸통 함수 호출
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glPushMatrix();// 처음 저장 좌표 다시 저장
Drawneck(); // 목함수 호출
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glPushMatrix();// 처음 저장 좌표 다시 저장
//머리 위치 설정
glRotatef(-75, 1, 0, 0); //x축기준으로 회전(머리를 위쪽으로 돌리기)
glTranslatef(0.0, -0.02, 0.0); //y축으로 이동 (머리 시작점)
DrawHead(); // 머리 호출
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glPushMatrix();// 처음 저장 좌표 다시 저장
//우측전체팔 위치 설정
DrawR_Arm((R_Arm_y + 30), 1, 0, 0); //우측팔호출
DrawR_Hand(-(R_Arm_y - 15), 1, 0, 0); //우측팔뚝
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glPushMatrix();// 처음 저장 좌표 다시 저장
//좌측전체팔 위치 설정
glTranslatef(0.0, -0.16, -0.04);//y축,z축으로 이동(좌측팔 시작점)
glRotatef(40, 0, 0, 1); //z축 기준으로 회전
DrawL_Arm((L_Arm_y + 30), 1, 0, 0); //좌측팔호출
DrawL_Hand(-(L_Arm_y - 15), 1, 0, 0); //좌측팔뚝
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glPushMatrix();// 처음 저장 좌표 다시 저장
//좌측전체 다리 위치 설정
glTranslatef(0.0, -0.45, -0.25);//y축,z축으로 이동(좌측다리 시작점)
glRotatef(-90, 1, 0, 0); //x축 기준으로 회전
DrawL_Legs(-30, 1, 0, 0); //좌측다리
DrawL_foot(10, 1, 0, 0); //좌측종아리
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glPushMatrix();// 처음 저장 좌표 다시 저장
//우측전체 다리 위치 설정
glTranslatef(0.0, -0.5, -0.5);//y축,z축으로 이동(우측다리 시작점)
glRotatef(-90, 1, 0, 0); //x축 기준으로 회전
DrawR_Legs(160, 1, 0, 0); //우측다리
DrawR_foot(R_Leg_y, 1, 0, 0); //우측종아리
glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
glutSwapBuffers();
}
IMPALGEBRA_BEGIN_NAMESPACE
Vector3D SphericalVector3D::get_cartesian_coordinates() {
return Vector3D(v_[0] * cos(v_[2]) * sin(v_[1]),
v_[0] * sin(v_[2]) * sin(v_[1]), v_[0] * cos(v_[1]));
}
void compute_projections(const Particles& all_particles,
const Particles& lig_particles,
unsigned int projection_number,
double pixel_size, double resolution,
boost::ptr_vector<Projection>& projections,
int image_size) {
// get coordinates and mass
IMP::algebra::Vector3Ds all_points(all_particles.size());
for (unsigned int i = 0; i < all_particles.size(); i++) {
all_points[i] = core::XYZ(all_particles[i]).get_coordinates();
}
IMP::algebra::Vector3Ds lig_points(lig_particles.size());
std::vector<double> lig_mass(lig_particles.size());
for (unsigned int i = 0; i < lig_particles.size(); i++) {
lig_points[i] = core::XYZ(lig_particles[i]).get_coordinates();
lig_mass[i] = atom::Mass(lig_particles[i]).get_mass();
}
int axis_size = image_size;
// use image_size if given
if (!(image_size > 0)) {
double max_dist = compute_max_distance(all_points);
static IMP::em::KernelParameters kp(resolution);
double wrap_length = 2 * kp.get_rkdist() + 1.0;
axis_size =
(int)((max_dist + 2 * wrap_length + 2 * pixel_size) / pixel_size + 2);
if (axis_size <= image_size) axis_size = image_size;
}
// storage for rotated points
IMP::algebra::Vector3Ds rotated_points(all_points.size());
IMP::algebra::Vector3Ds rotated_ligand_points(lig_points.size());
// points on a sphere
IMP::algebra::SphericalVector3Ds spherical_coords;
quasi_evenly_spherical_distribution(projection_number, spherical_coords, 1.0);
for (unsigned int i = 0; i < spherical_coords.size(); i++) {
// convert sphere coordinate to rotation
IMP::algebra::SphericalVector3D v = spherical_coords[i];
double cy = cos(v[1] / 2.0);
double cz = cos(v[2] / 2.0);
double sy = sin(v[1] / 2.0);
double sz = sin(v[2] / 2.0);
// this is a rotation about z axis by an angle v[2]
// followed by rotation about y axis by an angle v[1]
IMP::algebra::Rotation3D r(cy * cz, sy * sz, sy * cz, cy * sz);
// rotate points
for (unsigned int p_index = 0; p_index < all_points.size(); p_index++)
rotated_points[p_index] = r * all_points[p_index];
for (unsigned int p_index = 0; p_index < lig_points.size(); p_index++)
rotated_ligand_points[p_index] = r * lig_points[p_index];
// project
IMP_UNIQUE_PTR<Projection> p(
new Projection(rotated_points, rotated_ligand_points,
lig_mass, pixel_size, resolution, axis_size));
p->set_rotation(r);
p->set_axis(IMP::algebra::Vector3D(v.get_cartesian_coordinates()));
p->set_id(i);
// rasmol
// std::cout << "#projection " << i+1 <<"\nreset\nrotate Z "
//<< IMP_RAD_2_DEG(v[2]);
// std::cout << "\nrotate Y -" << IMP_RAD_2_DEG(v[1]) << std::endl;
// chimera
// std::cout << "#projection " << i+1
//<<"\nreset;turn x 180; turn z -" << IMP_RAD_2_DEG(v[2]);
// std::cout << ";turn y -" << IMP_RAD_2_DEG(v[1])
//<< ";wait;sleep 3;"<< std::endl;
// string file_name = "projection_" +
// string(boost::lexical_cast<string>(i+1)) + ".pgm";
// p.write_PGM(file_name);
projections.push_back(p.release());
}
}
QString PageItem_OSGFrame::getPDFMatrix(QString viewName)
{
struct viewDefinition defaultView;
defaultView = viewMap[viewName];
osg::Vec3d trackerCenter = defaultView.trackerCenter;
osg::Vec3d cameraPosition = defaultView.cameraPosition;
osg::Vec3d cameraUp = defaultView.cameraUp;
double viewx, viewy, viewz;
double leftx, lefty, leftz;
double upx, upy, upz;
double transx, transy, transz;
double roll = 0.0;
double roo = getDistance(cameraPosition, trackerCenter);
cameraPosition.normalize();
cameraUp.normalize();
viewx = -cameraPosition[0];
viewy = -cameraPosition[1];
viewz = -cameraPosition[2];
if (viewx == 0.0 && viewy == 0.0 && viewz == 0.0)
{
viewy = 1.0;
}
leftx = -1.0f;
lefty = 0.0f;
leftz = 0.0f;
if (viewz < 0.0) /* top view*/
{
upx = 0.0f;
upy = 1.0f;
upz = 0.0f;
}
else /* bottom view*/
{
upx = 0.0f;
upy =-1.0f;
upz = 0.0f;
}
if ( fabs(viewx) + fabs(viewy) != 0.0f) /* other views than top and bottom*/
{
cameraUp.normalize();
upx = cameraUp[0];
upy = cameraUp[1];
upz = cameraUp[2];
leftx = viewz*upy - viewy*upz;
lefty = viewx*upz - viewz*upx;
leftz = viewy*upx - viewx*upy;
normalized(leftx, lefty, leftz);
}
/* apply camera roll*/
{
double leftxprime, leftyprime, leftzprime;
double upxprime, upyprime, upzprime;
double sinroll, cosroll;
sinroll = sin((roll/180.0f)*M_PI);
cosroll = cos((roll/180.0f)*M_PI);
leftxprime = leftx*cosroll + upx*sinroll;
leftyprime = lefty*cosroll + upy*sinroll;
leftzprime = leftz*cosroll + upz*sinroll;
upxprime = upx*cosroll + leftx*sinroll;
upyprime = upy*cosroll + lefty*sinroll;
upzprime = upz*cosroll + leftz*sinroll;
leftx = leftxprime;
lefty = leftyprime;
leftz = leftzprime;
upx = upxprime;
upy = upyprime;
upz = upzprime;
}
/* translation vector*/
roo = fabs(roo);
if (roo == 0.0)
{
roo = 0.000000000000000001;
}
transx = trackerCenter[0] - roo*viewx;
transy = trackerCenter[1] - roo*viewy;
transz = trackerCenter[2] - roo*viewz;
if (fabs(leftx) < 0.0000001)
leftx = 0.0;
if (fabs(lefty) < 0.0000001)
lefty = 0.0;
if (fabs(leftz) < 0.0000001)
leftz = 0.0;
if (fabs(upx) < 0.0000001)
upx = 0.0;
if (fabs(upy) < 0.0000001)
upy = 0.0;
if (fabs(upz) < 0.0000001)
upz = 0.0;
if (fabs(transx) < 0.0000001)
transx = 0.0;
if (fabs(transy) < 0.0000001)
transy = 0.0;
if (fabs(transz) < 0.0000001)
transz = 0.0;
QString ret = "";
QString desc4 = "%1 %2 %3 ";
ret += desc4.arg(leftx).arg(lefty).arg(leftz);
ret += desc4.arg(upx).arg(upy).arg(upz);
ret += desc4.arg(viewx).arg(viewy).arg(viewz);
ret += desc4.arg(transx).arg(transy).arg(transz);
distanceToObj = roo;
return ret;
}
void CGameView::Update(const float deltaRealTime)
{
// If camera movement is being handled by the touch-input system,
// then we should stop to avoid conflicting with it
if (g_TouchInput.IsEnabled())
return;
if (!g_app_has_focus)
return;
if (m->TrackManager.IsActive() && m->TrackManager.IsPlaying())
{
if (! m->TrackManager.Update(deltaRealTime))
{
// ResetCamera();
}
return;
}
// Calculate mouse movement
static int mouse_last_x = 0;
static int mouse_last_y = 0;
int mouse_dx = g_mouse_x - mouse_last_x;
int mouse_dy = g_mouse_y - mouse_last_y;
mouse_last_x = g_mouse_x;
mouse_last_y = g_mouse_y;
if (HotkeyIsPressed("camera.rotate.cw"))
m->RotateY.AddSmoothly(m->ViewRotateYSpeed * deltaRealTime);
if (HotkeyIsPressed("camera.rotate.ccw"))
m->RotateY.AddSmoothly(-m->ViewRotateYSpeed * deltaRealTime);
if (HotkeyIsPressed("camera.rotate.up"))
m->RotateX.AddSmoothly(-m->ViewRotateXSpeed * deltaRealTime);
if (HotkeyIsPressed("camera.rotate.down"))
m->RotateX.AddSmoothly(m->ViewRotateXSpeed * deltaRealTime);
float moveRightward = 0.f;
float moveForward = 0.f;
if (HotkeyIsPressed("camera.pan"))
{
moveRightward += m->ViewDragSpeed * mouse_dx;
moveForward += m->ViewDragSpeed * -mouse_dy;
}
if (g_mouse_active)
{
if (g_mouse_x >= g_xres - 2 && g_mouse_x < g_xres)
moveRightward += m->ViewScrollSpeed * deltaRealTime;
else if (g_mouse_x <= 3 && g_mouse_x >= 0)
moveRightward -= m->ViewScrollSpeed * deltaRealTime;
if (g_mouse_y >= g_yres - 2 && g_mouse_y < g_yres)
moveForward -= m->ViewScrollSpeed * deltaRealTime;
else if (g_mouse_y <= 3 && g_mouse_y >= 0)
moveForward += m->ViewScrollSpeed * deltaRealTime;
}
if (HotkeyIsPressed("camera.right"))
moveRightward += m->ViewScrollSpeed * deltaRealTime;
if (HotkeyIsPressed("camera.left"))
moveRightward -= m->ViewScrollSpeed * deltaRealTime;
if (HotkeyIsPressed("camera.up"))
moveForward += m->ViewScrollSpeed * deltaRealTime;
if (HotkeyIsPressed("camera.down"))
moveForward -= m->ViewScrollSpeed * deltaRealTime;
if (g_Joystick.IsEnabled())
{
// This could all be improved with extra speed and sensitivity settings
// (maybe use pow to allow finer control?), and inversion settings
moveRightward += g_Joystick.GetAxisValue(m->JoystickPanX) * m->ViewScrollSpeed * deltaRealTime;
moveForward -= g_Joystick.GetAxisValue(m->JoystickPanY) * m->ViewScrollSpeed * deltaRealTime;
m->RotateX.AddSmoothly(g_Joystick.GetAxisValue(m->JoystickRotateX) * m->ViewRotateXSpeed * deltaRealTime);
m->RotateY.AddSmoothly(-g_Joystick.GetAxisValue(m->JoystickRotateY) * m->ViewRotateYSpeed * deltaRealTime);
// Use a +1 bias for zoom because I want this to work with trigger buttons that default to -1
m->Zoom.AddSmoothly((g_Joystick.GetAxisValue(m->JoystickZoomIn) + 1.0f) / 2.0f * m->ViewZoomSpeed * deltaRealTime);
m->Zoom.AddSmoothly(-(g_Joystick.GetAxisValue(m->JoystickZoomOut) + 1.0f) / 2.0f * m->ViewZoomSpeed * deltaRealTime);
}
if (moveRightward || moveForward)
{
// Break out of following mode when the user starts scrolling
m->FollowEntity = INVALID_ENTITY;
float s = sin(m->RotateY.GetSmoothedValue());
float c = cos(m->RotateY.GetSmoothedValue());
m->PosX.AddSmoothly(c * moveRightward);
m->PosZ.AddSmoothly(-s * moveRightward);
m->PosX.AddSmoothly(s * moveForward);
m->PosZ.AddSmoothly(c * moveForward);
}
if (m->FollowEntity)
{
CmpPtr<ICmpPosition> cmpPosition(*(m->Game->GetSimulation2()), m->FollowEntity);
CmpPtr<ICmpRangeManager> cmpRangeManager(*(m->Game->GetSimulation2()), SYSTEM_ENTITY);
if (cmpPosition && cmpPosition->IsInWorld() &&
cmpRangeManager && cmpRangeManager->GetLosVisibility(m->FollowEntity, m->Game->GetPlayerID(), false) == ICmpRangeManager::VIS_VISIBLE)
{
// Get the most recent interpolated position
float frameOffset = m->Game->GetSimulation2()->GetLastFrameOffset();
CMatrix3D transform = cmpPosition->GetInterpolatedTransform(frameOffset, false);
CVector3D pos = transform.GetTranslation();
if (m->FollowFirstPerson)
{
float x, z, angle;
cmpPosition->GetInterpolatedPosition2D(frameOffset, x, z, angle);
float height = 4.f;
m->ViewCamera.m_Orientation.SetIdentity();
m->ViewCamera.m_Orientation.RotateX((float)M_PI/24.f);
m->ViewCamera.m_Orientation.RotateY(angle);
m->ViewCamera.m_Orientation.Translate(pos.X, pos.Y + height, pos.Z);
m->ViewCamera.UpdateFrustum();
return;
}
else
{
// Move the camera to match the unit
CCamera targetCam = m->ViewCamera;
SetupCameraMatrixSmoothRot(m, &targetCam.m_Orientation);
CVector3D pivot = GetSmoothPivot(targetCam);
CVector3D delta = pos - pivot;
m->PosX.AddSmoothly(delta.X);
m->PosY.AddSmoothly(delta.Y);
m->PosZ.AddSmoothly(delta.Z);
}
}
else
{
// The unit disappeared (died or garrisoned etc), so stop following it
m->FollowEntity = INVALID_ENTITY;
}
}
if (HotkeyIsPressed("camera.zoom.in"))
m->Zoom.AddSmoothly(-m->ViewZoomSpeed * deltaRealTime);
if (HotkeyIsPressed("camera.zoom.out"))
m->Zoom.AddSmoothly(m->ViewZoomSpeed * deltaRealTime);
if (m->ConstrainCamera)
m->Zoom.ClampSmoothly(m->ViewZoomMin, m->ViewZoomMax);
float zoomDelta = -m->Zoom.Update(deltaRealTime);
if (zoomDelta)
{
CVector3D forwards = m->ViewCamera.m_Orientation.GetIn();
m->PosX.AddSmoothly(forwards.X * zoomDelta);
m->PosY.AddSmoothly(forwards.Y * zoomDelta);
m->PosZ.AddSmoothly(forwards.Z * zoomDelta);
}
if (m->ConstrainCamera)
m->RotateX.ClampSmoothly(DEGTORAD(m->ViewRotateXMin), DEGTORAD(m->ViewRotateXMax));
FocusHeight(m, true);
// Ensure the ViewCamera focus is inside the map with the chosen margins
// if not so - apply margins to the camera
if (m->ConstrainCamera)
{
CCamera targetCam = m->ViewCamera;
SetupCameraMatrixSmoothRot(m, &targetCam.m_Orientation);
CTerrain* pTerrain = m->Game->GetWorld()->GetTerrain();
CVector3D pivot = GetSmoothPivot(targetCam);
CVector3D delta = targetCam.m_Orientation.GetTranslation() - pivot;
CVector3D desiredPivot = pivot;
CmpPtr<ICmpRangeManager> cmpRangeManager(*(m->Game->GetSimulation2()), SYSTEM_ENTITY);
if (cmpRangeManager && cmpRangeManager->GetLosCircular())
{
// Clamp to a circular region around the center of the map
float r = pTerrain->GetMaxX() / 2;
CVector3D center(r, desiredPivot.Y, r);
float dist = (desiredPivot - center).Length();
if (dist > r - CAMERA_EDGE_MARGIN)
desiredPivot = center + (desiredPivot - center).Normalized() * (r - CAMERA_EDGE_MARGIN);
}
else
{
// Clamp to the square edges of the map
desiredPivot.X = Clamp(desiredPivot.X, pTerrain->GetMinX() + CAMERA_EDGE_MARGIN, pTerrain->GetMaxX() - CAMERA_EDGE_MARGIN);
desiredPivot.Z = Clamp(desiredPivot.Z, pTerrain->GetMinZ() + CAMERA_EDGE_MARGIN, pTerrain->GetMaxZ() - CAMERA_EDGE_MARGIN);
}
// Update the position so that pivot is within the margin
m->PosX.SetValueSmoothly(desiredPivot.X + delta.X);
m->PosZ.SetValueSmoothly(desiredPivot.Z + delta.Z);
}
m->PosX.Update(deltaRealTime);
m->PosY.Update(deltaRealTime);
m->PosZ.Update(deltaRealTime);
// Handle rotation around the Y (vertical) axis
{
CCamera targetCam = m->ViewCamera;
SetupCameraMatrixSmooth(m, &targetCam.m_Orientation);
float rotateYDelta = m->RotateY.Update(deltaRealTime);
if (rotateYDelta)
{
// We've updated RotateY, and need to adjust Pos so that it's still
// facing towards the original focus point (the terrain in the center
// of the screen).
CVector3D upwards(0.0f, 1.0f, 0.0f);
CVector3D pivot = GetSmoothPivot(targetCam);
CVector3D delta = targetCam.m_Orientation.GetTranslation() - pivot;
CQuaternion q;
q.FromAxisAngle(upwards, rotateYDelta);
CVector3D d = q.Rotate(delta) - delta;
m->PosX.Add(d.X);
m->PosY.Add(d.Y);
m->PosZ.Add(d.Z);
}
}
// Handle rotation around the X (sideways, relative to camera) axis
{
CCamera targetCam = m->ViewCamera;
SetupCameraMatrixSmooth(m, &targetCam.m_Orientation);
float rotateXDelta = m->RotateX.Update(deltaRealTime);
if (rotateXDelta)
{
CVector3D rightwards = targetCam.m_Orientation.GetLeft() * -1.0f;
CVector3D pivot = GetSmoothPivot(targetCam);
CVector3D delta = targetCam.m_Orientation.GetTranslation() - pivot;
CQuaternion q;
q.FromAxisAngle(rightwards, rotateXDelta);
CVector3D d = q.Rotate(delta) - delta;
m->PosX.Add(d.X);
m->PosY.Add(d.Y);
m->PosZ.Add(d.Z);
}
}
/* This is disabled since it doesn't seem necessary:
// Ensure the camera's near point is never inside the terrain
if (m->ConstrainCamera)
{
CMatrix3D target;
target.SetIdentity();
target.RotateX(m->RotateX.GetValue());
target.RotateY(m->RotateY.GetValue());
target.Translate(m->PosX.GetValue(), m->PosY.GetValue(), m->PosZ.GetValue());
CVector3D nearPoint = target.GetTranslation() + target.GetIn() * defaultNear;
float ground = m->Game->GetWorld()->GetTerrain()->GetExactGroundLevel(nearPoint.X, nearPoint.Z);
float limit = ground + 16.f;
if (nearPoint.Y < limit)
m->PosY.AddSmoothly(limit - nearPoint.Y);
}
*/
m->RotateY.Wrap(-(float)M_PI, (float)M_PI);
// Update the camera matrix
m->ViewCamera.SetProjection(m->ViewNear, m->ViewFar, m->ViewFOV);
SetupCameraMatrixSmooth(m, &m->ViewCamera.m_Orientation);
m->ViewCamera.UpdateFrustum();
}
void camera::moveCameraUp(const float& dir)
{
float rad=(camPitch+dir)*M_PI/180.0;
loc.v[1]+=sin(rad)*speed;
}
inline vector2 rotate(T angle) const
{
return vector2(x * cos(angle) - y * sin(angle), y * cos(angle) + x * sin(angle));
}
static void geod_for(void) {
double d,sind,u,V,X,ds,cosds,sinds,ss,de;
ellipse = 1;
ss = 0.;
if (ellipse) {
d = geod_S / (D * geod_a);
if (signS) d = -d;
u = 2. * (s1 - d);
V = cos(u + d);
X = c2 * c2 * (sind = sin(d)) * cos(d) * (2. * V * V - 1.);
ds = d + X - 2. * P * V * (1. - 2. * P * cos(u)) * sind;
ss = s1 + s1 - ds;
} else {
ds = geod_S / geod_a;
if (signS) ds = - ds;
}
cosds = cos(ds);
sinds = sin(ds);
if (signS) sinds = - sinds;
al21 = N * cosds - sinth1 * sinds;
if (merid) {
phi2 = atan( tan(HALFPI + s1 - ds) / onef);
if (al21 > 0.) {
al21 = PI;
if (signS)
de = PI;
else {
phi2 = - phi2;
de = 0.;
}
} else {
al21 = 0.;
if (signS) {
phi2 = - phi2;
de = 0;
} else
de = PI;
}
} else {
al21 = atan(M / al21);
if (al21 > 0)
al21 += PI;
if (al12 < 0.)
al21 -= PI;
al21 = adjlon(al21);
phi2 = atan(-(sinth1 * cosds + N * sinds) * sin(al21) /
(ellipse ? onef * M : M));
de = atan2(sinds * sina12 ,
(costh1 * cosds - sinth1 * sinds * cosa12));
if (ellipse){
if (signS)
de += c1 * ((1. - c2) * ds +
c2 * sinds * cos(ss));
else
de -= c1 * ((1. - c2) * ds -
c2 * sinds * cos(ss));
}
}
lam2 = adjlon( lam1 + de );
}
void AvatarGameObj::step_impl() {
dBodyID body = get_entity().get_id();
const Channel* chn;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
dBodyAddRelForce(body, -v, 0.0, 0.0);
}
bool pushing_up = false;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateY);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
if (Saving::get().config().invertTranslateY()) {
v = -v;
}
dBodyAddRelForce(body, 0.0, -v, 0.0);
if (v < 0) {
pushing_up = true;
}
}
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ACCEL/MAX_FPS);
dBodyAddRelForce(body, 0.0, 0.0, -v);
}
const dReal* avel = dBodyGetAngularVel(body);
dVector3 rel_avel;
dBodyVectorFromWorld(body, avel[0], avel[1], avel[2], rel_avel);
// X-turn and x-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateY);
if (chn->is_on()) {
float v = -(chn->get_value())*(MAX_TURN/MAX_FPS);
dBodyAddRelTorque(body, 0.0, v, 0.0);
} else {
float cv = rel_avel[1]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, 0.0, cv, 0.0);
}
// Y-turn and y-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_TURN/MAX_FPS);
if (Saving::get().config().invertRotateY()) {
v = -v;
}
dBodyAddRelTorque(body, v, 0.0, 0.0);
} else {
float cv = rel_avel[0]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, cv, 0.0, 0.0);
}
// Roll and counter-roll
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ROLL/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, v);
} else {
float cv = rel_avel[2]*(-CROLL_COEF/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, cv);
}
// Changing stance between superman-style and upright
if (_attached) {
_uprightness += UPRIGHTNESS_STEP_DIFF;
} else {
_uprightness -= UPRIGHTNESS_STEP_DIFF;
}
if (_uprightness > 1.0) { _uprightness = 1.0; } else if (_uprightness < 0.0) { _uprightness = 0.0; }
update_geom_offsets();
_attached = _attached_this_frame;
// If we are attached, work to keep ourselves ideally oriented to the attachment surface
if (_attached) {
Vector sn_rel = vector_from_world(_sn);
Vector lvel = Vector(dBodyGetLinearVel(body));
Vector lvel_rel = vector_from_world(lvel);
Vector avel = Vector(dBodyGetAngularVel(body));
Vector avel_rel = vector_from_world(avel);
// Apply as much of each delta as we can
// X and Z orientation delta
// TODO Maybe should translate body so that the contact point stays in the same spot through rotation
float a = limit_abs(_zrot_delta, RUNNING_ADJ_RATE_Z_ROT/MAX_FPS);
Vector body_x(vector_to_world(Vector(cos(a), sin(a), 0)));
a = limit_abs(-_xrot_delta, RUNNING_ADJ_RATE_X_ROT/MAX_FPS);
Vector body_y(vector_to_world(Vector(0, cos(a), sin(a))));
dMatrix3 matr;
dRFrom2Axes(matr, body_x.x, body_x.y, body_x.z, body_y.x, body_y.y, body_y.z);
dBodySetRotation(body, matr);
// Y position delta
// If the user is pushing up, set the target point high above the ground so we escape sticky attachment
set_pos(get_pos() + _sn*limit_abs(_ypos_delta + (pushing_up ? RUNNING_MAX_DELTA_Y_POS*2 : 0), RUNNING_ADJ_RATE_Y_POS/MAX_FPS));
// Y linear velocity delta
lvel_rel.y += limit_abs(_ylvel_delta, RUNNING_ADJ_RATE_Y_LVEL/MAX_FPS);
lvel = vector_to_world(lvel_rel);
dBodySetLinearVel(body, lvel.x, lvel.y, lvel.z);
// X and Z angular velocity delta
avel_rel.x += limit_abs(_xavel_delta, RUNNING_ADJ_RATE_X_AVEL/MAX_FPS);
avel_rel.z += limit_abs(_zavel_delta, RUNNING_ADJ_RATE_Z_AVEL/MAX_FPS);
avel = vector_to_world(avel_rel);
dBodySetAngularVel(body, avel.x, avel.y, avel.z);
}
if (_attached_this_frame) {
_attached_this_frame = false;
dGeomEnable(get_entity().get_geom("sticky_attach"));
} else {
dGeomDisable(get_entity().get_geom("sticky_attach"));
}
}
static void geod_inv() {
double th1,th2,thm,dthm,dlamm,dlam,sindlamm,costhm,sinthm,cosdthm,
sindthm,L,E,cosd,d,X,Y,T,sind,tandlammp,u,v,D,A,B;
/* Stuff the WGS84 projection parameters as necessary
To avoid having to include <geodesic,h>
*/
ellipse = 1;
f = 1.0 / WGSinvf; /* WGS84 ellipsoid flattening parameter */
geod_a = WGS84_semimajor_axis_meters;
es = 2 * f - f * f;
onef = sqrt(1. - es);
geod_f = 1 - onef;
f2 = geod_f/2;
f4 = geod_f/4;
f64 = geod_f*geod_f/64;
if (ellipse) {
th1 = atan(onef * tan(phi1));
th2 = atan(onef * tan(phi2));
} else {
th1 = phi1;
th2 = phi2;
}
thm = .5 * (th1 + th2);
dthm = .5 * (th2 - th1);
dlamm = .5 * ( dlam = adjlon(lam2 - lam1) );
if (fabs(dlam) < DTOL && fabs(dthm) < DTOL) {
al12 = al21 = geod_S = 0.;
return;
}
sindlamm = sin(dlamm);
costhm = cos(thm); sinthm = sin(thm);
cosdthm = cos(dthm); sindthm = sin(dthm);
L = sindthm * sindthm + (cosdthm * cosdthm - sinthm * sinthm)
* sindlamm * sindlamm;
d = acos(cosd = 1 - L - L);
if (ellipse) {
E = cosd + cosd;
sind = sin( d );
Y = sinthm * cosdthm;
Y *= (Y + Y) / (1. - L);
T = sindthm * costhm;
T *= (T + T) / L;
X = Y + T;
Y -= T;
T = d / sind;
D = 4. * T * T;
A = D * E;
B = D + D;
geod_S = geod_a * sind * (T - f4 * (T * X - Y) +
f64 * (X * (A + (T - .5 * (A - E)) * X) -
Y * (B + E * Y) + D * X * Y));
tandlammp = tan(.5 * (dlam - .25 * (Y + Y - E * (4. - X)) *
(f2 * T + f64 * (32. * T - (20. * T - A)
* X - (B + 4.) * Y)) * tan(dlam)));
} else {
geod_S = geod_a * d;
tandlammp = tan(dlamm);
}
u = atan2(sindthm , (tandlammp * costhm));
v = atan2(cosdthm , (tandlammp * sinthm));
al12 = adjlon(TWOPI + v - u);
al21 = adjlon(TWOPI - v - u);
}
/**
*@brief 更新
* @param st 刻み幅
*/
void RobotArm::update(double st)
{
if(pauseFalg || stopFalg || !serbo)
return;
//std::cout << targetPoint.end_time << "\t" << targetPoint.time << std::endl;
if(targetPoint.end_time < targetPoint.time)
{
if(targetPoint.type == Point)
{
Vector3d pos = calcKinematics(theta);
double dPx = Kp*(targetPoint.target_pos(0)-pos(0));
double dPy = Kp*(targetPoint.target_pos(1)-pos(1));
double dPz = Kp*(targetPoint.target_pos(2)-pos(2));
Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz));
updatePos(dthe(0), dthe(1), dthe(2), 0);
theta[3] = targetPoint.target_theta;
}
else
{
for(int i=0;i < 4;i++)
{
theta[i] = targetPoint.target_joint_pos[i];
}
if (!judgeSoftLimitJoint(theta))stop();
}
setTargetPos();
return;
}
else
{
if(targetPoint.type == Point)
{
double td = calcVel(targetPoint.target_theta, targetPoint.start_theta, targetPoint.end_time, targetPoint.time, theta[3]);
dt = st;
MinTime = dt;
double dx = targetPoint.target_pos(0)-targetPoint.start_pos(0);
double dy = targetPoint.target_pos(1)-targetPoint.start_pos(1);
double dz = targetPoint.target_pos(2)-targetPoint.start_pos(2);
double ST = sqrt(dx*dx+dy*dy+dz*dz);
if(ST < 0.001)
{
updatePos(0, 0, 0, td);
targetPoint.time += dt;
return;
}
double A = 2*M_PI*ST/(targetPoint.end_time*targetPoint.end_time);
double s = A*targetPoint.end_time/(2*M_PI)*(targetPoint.time - targetPoint.end_time/(2*M_PI)*sin(2*M_PI/targetPoint.end_time*targetPoint.time));
double Px = s*dx/ST + targetPoint.start_pos(0);
double Py = s*dy/ST + targetPoint.start_pos(1);
double Pz = s*dz/ST + targetPoint.start_pos(2);
double ds = A*targetPoint.end_time/(2*M_PI)*(1 - cos(2*M_PI/targetPoint.end_time*targetPoint.time));
Vector3d pos = calcKinematics(theta);
double dPx = ds*dx/ST + Kp*(Px-pos(0));
double dPy = ds*dy/ST + Kp*(Py-pos(1));
double dPz = ds*dz/ST + Kp*(Pz-pos(2));
//ofs << ds << "\t" << s << std::endl;
//std::cout << pos << std::endl;
Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz));
updatePos(dthe(0), dthe(1), dthe(2), td);
}
else
{
double dthe[4];
for(int i=0;i < 4;i++)
{
dthe[i] = calcVel(targetPoint.target_joint_pos[i], targetPoint.start_joint_pos[i], targetPoint.end_time, targetPoint.time, theta[i]);
}
updatePos(dthe[0], dthe[1], dthe[2], dthe[3]);
}
targetPoint.time += dt;
}
}
inline Real operator() (Real t, Real x, Real y, Real z, UInt /*ic*/ = 0) const
{
return exp (-sin (Pi / 2 * t) ) * (x + y + z);
}
void Camera::update(LocalPlayer* player, f32 frametime, v2u32 screensize,
f32 tool_reload_ratio)
{
// Get player position
// Smooth the movement when walking up stairs
v3f old_player_position = m_playernode->getPosition();
v3f player_position = player->getPosition();
if (player->isAttached && player->parent)
player_position = player->parent->getPosition();
//if(player->touching_ground && player_position.Y > old_player_position.Y)
if(player->touching_ground &&
player_position.Y > old_player_position.Y)
{
f32 oldy = old_player_position.Y;
f32 newy = player_position.Y;
f32 t = exp(-23*frametime);
player_position.Y = oldy * t + newy * (1-t);
}
// Set player node transformation
m_playernode->setPosition(player_position);
m_playernode->setRotation(v3f(0, -1 * player->getYaw(), 0));
m_playernode->updateAbsolutePosition();
// Get camera tilt timer (hurt animation)
float cameratilt = fabs(fabs(player->hurt_tilt_timer-0.75)-0.75);
// Fall bobbing animation
float fall_bobbing = 0;
if(player->camera_impact >= 1)
{
if(m_view_bobbing_fall == -1) // Effect took place and has finished
player->camera_impact = m_view_bobbing_fall = 0;
else if(m_view_bobbing_fall == 0) // Initialize effect
m_view_bobbing_fall = 1;
// Convert 0 -> 1 to 0 -> 1 -> 0
fall_bobbing = m_view_bobbing_fall < 0.5 ? m_view_bobbing_fall * 2 : -(m_view_bobbing_fall - 0.5) * 2 + 1;
// Smoothen and invert the above
fall_bobbing = sin(fall_bobbing * 0.5 * M_PI) * -1;
// Amplify according to the intensity of the impact
fall_bobbing *= (1 - rangelim(50 / player->camera_impact, 0, 1)) * 5;
fall_bobbing *= g_settings->getFloat("fall_bobbing_amount");
}
// Set head node transformation
m_headnode->setPosition(player->getEyeOffset()+v3f(0,cameratilt*-player->hurt_tilt_strength+fall_bobbing,0));
m_headnode->setRotation(v3f(player->getPitch(), 0, cameratilt*player->hurt_tilt_strength));
m_headnode->updateAbsolutePosition();
// Compute relative camera position and target
v3f rel_cam_pos = v3f(0,0,0);
v3f rel_cam_target = v3f(0,0,1);
v3f rel_cam_up = v3f(0,1,0);
// Compute absolute camera position and target
m_headnode->getAbsoluteTransformation().transformVect(m_camera_position, rel_cam_pos);
m_headnode->getAbsoluteTransformation().rotateVect(m_camera_direction, rel_cam_target - rel_cam_pos);
v3f abs_cam_up;
m_headnode->getAbsoluteTransformation().rotateVect(abs_cam_up, rel_cam_up);
// Set camera node transformation
m_cameranode->setPosition(m_camera_position);
//m_cameranode->setUpVector(abs_cam_up);
// *100.0 helps in large map coordinates
//m_cameranode->setTarget(m_camera_position + 100 * m_camera_direction);
m_cameranode->setTarget(m_camera_position+v3f(-1,-1,1));
// Get FOV setting
f32 fov_degrees = g_settings->getFloat("fov");
fov_degrees = MYMAX(fov_degrees, 10.0);
fov_degrees = MYMIN(fov_degrees, 170.0);
// FOV and aspect ratio
m_aspect = (f32)screensize.X / (f32) screensize.Y;
m_fov_y = fov_degrees * M_PI / 180.0;
// Increase vertical FOV on lower aspect ratios (<16:10)
m_fov_y *= MYMAX(1.0, MYMIN(1.4, sqrt(16./10. / m_aspect)));
m_fov_x = 2 * atan(m_aspect * tan(0.5 * m_fov_y));
/*m_cameranode->setAspectRatio(m_aspect);
m_cameranode->setFOV(m_fov_y);*/
// Position the wielded item
//v3f wield_position = v3f(45, -35, 65);
v3f wield_position = v3f(55, -35, 65);
//v3f wield_rotation = v3f(-100, 120, -100);
v3f wield_rotation = v3f(-100, 120, -100);
if(m_wield_change_timer < 0)
wield_position.Y -= 40 + m_wield_change_timer*320;
else
wield_position.Y -= 40 - m_wield_change_timer*320;
if(m_digging_anim < 0.05 || m_digging_anim > 0.5){
f32 frac = 1.0;
if(m_digging_anim > 0.5)
frac = 2.0 * (m_digging_anim - 0.5);
// This value starts from 1 and settles to 0
f32 ratiothing = pow((1.0f - tool_reload_ratio), 0.5f);
//f32 ratiothing2 = pow(ratiothing, 0.5f);
f32 ratiothing2 = (easeCurve(ratiothing*0.5))*2.0;
wield_position.Y -= frac * 25.0 * pow(ratiothing2, 1.7f);
//wield_position.Z += frac * 5.0 * ratiothing2;
wield_position.X -= frac * 35.0 * pow(ratiothing2, 1.1f);
wield_rotation.Y += frac * 70.0 * pow(ratiothing2, 1.4f);
//wield_rotation.X -= frac * 15.0 * pow(ratiothing2, 1.4f);
//wield_rotation.Z += frac * 15.0 * pow(ratiothing2, 1.0f);
}
if (m_digging_button != -1)
{
f32 digfrac = m_digging_anim;
wield_position.X -= 50 * sin(pow(digfrac, 0.8f) * M_PI);
wield_position.Y += 24 * sin(digfrac * 1.8 * M_PI);
wield_position.Z += 25 * 0.5;
// Euler angles are PURE EVIL, so why not use quaternions?
core::quaternion quat_begin(wield_rotation * core::DEGTORAD);
core::quaternion quat_end(v3f(80, 30, 100) * core::DEGTORAD);
core::quaternion quat_slerp;
quat_slerp.slerp(quat_begin, quat_end, sin(digfrac * M_PI));
quat_slerp.toEuler(wield_rotation);
wield_rotation *= core::RADTODEG;
}
else {
f32 bobfrac = my_modf(m_view_bobbing_anim);
wield_position.X -= sin(bobfrac*M_PI*2.0) * 3.0;
wield_position.Y += sin(my_modf(bobfrac*2.0)*M_PI) * 3.0;
}
m_wieldnode->setPosition(wield_position);
m_wieldnode->setRotation(wield_rotation);
m_wieldlight = player->light;
// Render distance feedback loop
updateViewingRange(frametime);
// If the player seems to be walking on solid ground,
// view bobbing is enabled and free_move is off,
// start (or continue) the view bobbing animation.
v3f speed = player->getSpeed();
if ((hypot(speed.X, speed.Z) > BS) &&
(player->touching_ground) &&
(g_settings->getBool("view_bobbing") == true) &&
(g_settings->getBool("free_move") == false ||
!m_gamedef->checkLocalPrivilege("fly")))
{
// Start animation
m_view_bobbing_state = 1;
m_view_bobbing_speed = MYMIN(speed.getLength(), 40);
}
else if (m_view_bobbing_state == 1)
{
// Stop animation
m_view_bobbing_state = 2;
m_view_bobbing_speed = 60;
}
}
void
Point::rotate(double angle, Point* center)
{
double cur_x = (double)this->x;
double cur_y = (double)this->y;
this->x = (long)round( (double)center->x + cos(angle) * (cur_x - (double)center->x) - sin(angle) * (cur_y - (double)center->y) );
this->y = (long)round( (double)center->y + cos(angle) * (cur_y - (double)center->y) + sin(angle) * (cur_x - (double)center->x) );
}
nav_msgs::Odometry OdometryFactory::getOdometryData() {
nav_msgs::Odometry odometry_message;
if (sequence_id == 0) {
last_time = ros::Time::now();
}
current_time = ros::Time::now();
duration = current_time - last_time;
position_x += velocity_x * sin(yaw) * duration.toSec();
position_y += velocity_x * cos(yaw) * duration.toSec();
std::cout<<"position)_y "<<position_y<<" velocity_x "<<velocity_x<<" duration "<<duration.toSec()<<" cos(yaw) "<<cos(yaw)<<std::endl;
yaw += yaw_rate * duration.toSec();
quaternion = tf::createQuaternionMsgFromYaw(yaw);
//tf update
geometry_msgs::TransformStamped transform_stamped;
transform_stamped.header.stamp = current_time;
transform_stamped.header.frame_id = "odom";
transform_stamped.child_frame_id = "base_footprint";
transform_stamped.transform.translation.x = position_x;
transform_stamped.transform.translation.y = position_y;
transform_stamped.transform.translation.z = 0;
transform_stamped.transform.rotation = quaternion;
//tf broadcast
odom_broadcaster.sendTransform(transform_stamped);
//Odometry message update
//Header
odometry_message.header.seq = sequence_id++;
odometry_message.header.stamp = current_time;
odometry_message.header.frame_id = "odom";
//Child Frame
odometry_message.child_frame_id = "base_link";
//Twist
odometry_message.twist.twist.linear.x = velocity_x;
odometry_message.twist.twist.linear.y = 0; //Fixed
odometry_message.twist.twist.linear.z = 0; //Fixed
odometry_message.twist.twist.angular.x = 0; //Fixed
odometry_message.twist.twist.angular.y = 0; //Fixed
odometry_message.twist.twist.angular.z = yaw_rate;
for (int i = 0; i < 36; i++) {
odometry_message.twist.covariance[i] = twist_covariance_matrix[i];
}
//Pose
odometry_message.pose.pose.position.x = position_x;
odometry_message.pose.pose.position.y = position_y;
odometry_message.pose.pose.position.z = 0; //Fixed
odometry_message.pose.pose.orientation = quaternion;
for (int i = 0; i < 36; i++) {
odometry_message.pose.covariance[i] = pose_covariance_matrix[i];
}
last_time = current_time;
return odometry_message;
}
/**
* G26: Mesh Validation Pattern generation.
*
* Used to interactively edit UBL's Mesh by placing the
* nozzle in a problem area and doing a G29 P4 R command.
*/
void gcode_G26() {
SERIAL_ECHOLNPGM("G26 command started. Waiting for heater(s).");
float tmp, start_angle, end_angle;
int i, xi, yi;
mesh_index_pair location;
// Don't allow Mesh Validation without homing first,
// or if the parameter parsing did not go OK, abort
if (axis_unhomed_error(true, true, true) || parse_G26_parameters()) return;
if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) {
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
stepper.synchronize();
set_current_to_destination();
}
if (turn_on_heaters()) goto LEAVE;
current_position[E_AXIS] = 0.0;
sync_plan_position_e();
if (prime_flag && prime_nozzle()) goto LEAVE;
/**
* Bed is preheated
*
* Nozzle is at temperature
*
* Filament is primed!
*
* It's "Show Time" !!!
*/
ZERO(circle_flags);
ZERO(horizontal_mesh_line_flags);
ZERO(vertical_mesh_line_flags);
// Move nozzle to the specified height for the first layer
set_destination_to_current();
destination[Z_AXIS] = layer_height;
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0.0);
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], ooze_amount);
ubl.has_control_of_lcd_panel = true;
//debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern."));
/**
* Declare and generate a sin() & cos() table to be used during the circle drawing. This will lighten
* the CPU load and make the arc drawing faster and more smooth
*/
float sin_table[360 / 30 + 1], cos_table[360 / 30 + 1];
for (i = 0; i <= 360 / 30; i++) {
cos_table[i] = SIZE_OF_INTERSECTION_CIRCLES * cos(RADIANS(valid_trig_angle(i * 30.0)));
sin_table[i] = SIZE_OF_INTERSECTION_CIRCLES * sin(RADIANS(valid_trig_angle(i * 30.0)));
}
do {
if (ubl_lcd_clicked()) { // Check if the user wants to stop the Mesh Validation
#if ENABLED(ULTRA_LCD)
lcd_setstatuspgm(PSTR("Mesh Validation Stopped."), 99);
lcd_quick_feedback();
#endif
while (!ubl_lcd_clicked()) { // Wait until the user is done pressing the
idle(); // Encoder Wheel if that is why we are leaving
lcd_reset_alert_level();
lcd_setstatuspgm(PSTR(""));
}
while (ubl_lcd_clicked()) { // Wait until the user is done pressing the
idle(); // Encoder Wheel if that is why we are leaving
lcd_setstatuspgm(PSTR("Unpress Wheel"), 99);
}
goto LEAVE;
}
location = continue_with_closest
? find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS])
: find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now.
if (location.x_index >= 0 && location.y_index >= 0) {
const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem
#ifdef DELTA
if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to print outside of DELTA_PRINTABLE_RADIUS.");
goto LEAVE;
}
#endif
// TODO: Change this to use `position_is_reachable`
if (!WITHIN(circle_x, X_MIN_POS, X_MAX_POS) || !WITHIN(circle_y, Y_MIN_POS, Y_MAX_POS)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to print off the bed.");
goto LEAVE;
}
xi = location.x_index; // Just to shrink the next few lines and make them easier to understand
yi = location.y_index;
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Doing circle at: (xi=", xi);
SERIAL_ECHOPAIR(", yi=", yi);
SERIAL_CHAR(')');
SERIAL_EOL;
}
start_angle = 0.0; // assume it is going to be a full circle
end_angle = 360.0;
if (xi == 0) { // Check for bottom edge
start_angle = -90.0;
end_angle = 90.0;
if (yi == 0) // it is an edge, check for the two left corners
start_angle = 0.0;
else if (yi == GRID_MAX_POINTS_Y - 1)
end_angle = 0.0;
}
else if (xi == GRID_MAX_POINTS_X - 1) { // Check for top edge
start_angle = 90.0;
end_angle = 270.0;
if (yi == 0) // it is an edge, check for the two right corners
end_angle = 180.0;
else if (yi == GRID_MAX_POINTS_Y - 1)
start_angle = 180.0;
}
else if (yi == 0) {
start_angle = 0.0; // only do the top side of the cirlce
end_angle = 180.0;
}
else if (yi == GRID_MAX_POINTS_Y - 1) {
start_angle = 180.0; // only do the bottom side of the cirlce
end_angle = 360.0;
}
for (tmp = start_angle; tmp < end_angle - 0.1; tmp += 30.0) {
int tmp_div_30 = tmp / 30.0;
if (tmp_div_30 < 0) tmp_div_30 += 360 / 30;
if (tmp_div_30 > 11) tmp_div_30 -= 360 / 30;
float x = circle_x + cos_table[tmp_div_30], // for speed, these are now a lookup table entry
y = circle_y + sin_table[tmp_div_30],
xe = circle_x + cos_table[tmp_div_30 + 1],
ye = circle_y + sin_table[tmp_div_30 + 1];
#ifdef DELTA
if (HYPOT2(x, y) > sq(DELTA_PRINTABLE_RADIUS)) // Check to make sure this part of
continue; // the 'circle' is on the bed. If
#else // not, we need to skip
x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1);
xe = constrain(xe, X_MIN_POS + 1, X_MAX_POS - 1);
ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1);
#endif
//if (ubl.g26_debug_flag) {
// char ccc, *cptr, seg_msg[50], seg_num[10];
// strcpy(seg_msg, " segment: ");
// strcpy(seg_num, " \n");
// cptr = (char*) "01234567890ABCDEF????????";
// ccc = cptr[tmp_div_30];
// seg_num[1] = ccc;
// strcat(seg_msg, seg_num);
// debug_current_and_destination(seg_msg);
//}
print_line_from_here_to_there(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), layer_height, LOGICAL_X_POSITION(xe), LOGICAL_Y_POSITION(ye), layer_height);
}
//debug_current_and_destination(PSTR("Looking for lines to connect."));
look_for_lines_to_connect();
//debug_current_and_destination(PSTR("Done with line connect."));
}
//debug_current_and_destination(PSTR("Done with current circle."));
} while (location.x_index >= 0 && location.y_index >= 0);
LEAVE:
lcd_reset_alert_level();
lcd_setstatuspgm(PSTR("Leaving G26"));
retract_filament(destination);
destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
//debug_current_and_destination(PSTR("ready to do Z-Raise."));
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle
//debug_current_and_destination(PSTR("done doing Z-Raise."));
destination[X_AXIS] = x_pos; // Move back to the starting position
destination[Y_AXIS] = y_pos;
//destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position
//debug_current_and_destination(PSTR("done doing X/Y move."));
ubl.has_control_of_lcd_panel = false; // Give back control of the LCD Panel!
if (!keep_heaters_on) {
#if HAS_TEMP_BED
thermalManager.setTargetBed(0);
#endif
thermalManager.setTargetHotend(0, 0);
}
}
double *r8vec_normal_01_new ( int n, int *seed )
/******************************************************************************/
/*
Purpose:
R8VEC_NORMAL_01_NEW returns a unit pseudonormal R8VEC.
Discussion:
The standard normal probability distribution function (PDF) has
mean 0 and standard deviation 1.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
06 August 2013
Author:
John Burkardt
Parameters:
Input, int N, the number of values desired.
Input/output, int *SEED, a seed for the random number generator.
Output, double R8VEC_NORMAL_01_NEW[N], a sample of the standard normal PDF.
Local parameters:
Local, double R[N+1], is used to store some uniform random values.
Its dimension is N+1, but really it is only needed to be the
smallest even number greater than or equal to N.
Local, int X_LO, X_HI, records the range of entries of
X that we need to compute.
*/
{
int i;
int m;
const double pi = 3.141592653589793;
double *r;
double *x;
int x_hi;
int x_lo;
x = ( double * ) malloc ( n * sizeof ( double ) );
/*
Record the range of X we need to fill in.
*/
x_lo = 1;
x_hi = n;
/*
If we need just one new value, do that here to avoid null arrays.
*/
if ( x_hi - x_lo + 1 == 1 )
{
r = r8vec_uniform_01_new ( 2, seed );
x[x_hi-1] = sqrt ( - 2.0 * log ( r[0] ) ) * cos ( 2.0 * pi * r[1] );
free ( r );
}
/*
If we require an even number of values, that's easy.
*/
else if ( ( x_hi - x_lo + 1 ) % 2 == 0 )
{
m = ( x_hi - x_lo + 1 ) / 2;
r = r8vec_uniform_01_new ( 2*m, seed );
for ( i = 0; i <= 2*m-2; i = i + 2 )
{
x[x_lo+i-1] = sqrt ( - 2.0 * log ( r[i] ) ) * cos ( 2.0 * pi * r[i+1] );
x[x_lo+i ] = sqrt ( - 2.0 * log ( r[i] ) ) * sin ( 2.0 * pi * r[i+1] );
}
free ( r );
}
/*
If we require an odd number of values, we generate an even number,
and handle the last pair specially, storing one in X(N).
*/
else
{
x_hi = x_hi - 1;
m = ( x_hi - x_lo + 1 ) / 2 + 1;
r = r8vec_uniform_01_new ( 2*m, seed );
for ( i = 0; i <= 2*m-4; i = i + 2 )
{
x[x_lo+i-1] = sqrt ( - 2.0 * log ( r[i] ) ) * cos ( 2.0 * pi * r[i+1] );
x[x_lo+i ] = sqrt ( - 2.0 * log ( r[i] ) ) * sin ( 2.0 * pi * r[i+1] );
}
i = 2*m - 2;
x[x_lo+i-1] = sqrt ( - 2.0 * log ( r[i] ) ) * cos ( 2.0 * pi * r[i+1] );
free ( r );
}
return x;
}
/*
=================
ParseBrush
=================
*/
static void ParseBrush (void)
{
mbrush_t *b;
mface_t *f, *f2;
vec3_t planepts[3];
vec3_t t1, t2, t3;
int i, j;
texinfo_t tx;
double d;
float shift[2], rotate, scale[2];
char name[64];
b = &mapbrushes[nummapbrushes];
do
{
if (!GetToken (true))
break;
if (!strcmp (token, "}") )
break;
// read the three point plane definition
for (i = 0 ; i < 3 ; i++)
{
if (i != 0)
GetToken (true);
if (strcmp (token, "(") )
Error ("parsing brush");
for (j = 0 ; j < 3 ; j++)
{
GetToken (false);
planepts[i][j] = atoi(token);
}
GetToken (false);
if (strcmp (token, ")") )
Error ("parsing brush");
}
// read the texturedef
memset (&tx, 0, sizeof(tx));
GetToken (false);
strcpy (name, token); // JDC 8/8/97: for origin texture
tx.miptex = FindMiptex (token);
GetToken (false);
shift[0] = atoi(token);
GetToken (false);
shift[1] = atoi(token);
GetToken (false);
rotate = atoi(token);
GetToken (false);
scale[0] = atof(token);
GetToken (false);
scale[1] = atof(token);
GetToken (false);
b->Light = atoi(token);
// if the three points are all on a previous plane, it is a
// duplicate plane
for (f2 = b->faces ; f2 ; f2 = f2->next)
{
for (i = 0 ; i < 3 ; i++)
{
d = DotProduct(planepts[i],f2->plane.normal) - f2->plane.dist;
if (d < -ON_EPSILON || d > ON_EPSILON)
break;
}
if (i == 3)
break;
}
if (f2)
{
printf ("WARNING: brush with duplicate plane\n");
continue;
}
f = (mface_t *) malloc(sizeof(mface_t));
f->next = b->faces;
b->faces = f;
// convert to a vector / dist plane
for (j = 0 ; j < 3 ; j++)
{
t1[j] = planepts[0][j] - planepts[1][j];
t2[j] = planepts[2][j] - planepts[1][j];
t3[j] = planepts[1][j];
}
CrossProduct(t1,t2, f->plane.normal);
if (VectorCompare (f->plane.normal, vec3_origin))
{
printf ("WARNING: brush plane with no normal\n");
b->faces = f->next;
free (f);
break;
}
VectorNormalize (f->plane.normal);
f->plane.dist = DotProduct (t3, f->plane.normal);
//
// fake proper texture vectors from QuakeEd style
//
{
vec3_t vecs[2];
int sv, tv;
float ang, sinv, cosv;
float ns, nt;
TextureAxisFromPlane(&f->plane, vecs[0], vecs[1]);
if (!scale[0])
scale[0] = 1;
if (!scale[1])
scale[1] = 1;
// rotate axis
if (rotate == 0)
{
sinv = 0;
cosv = 1;
}
else if (rotate == 90)
{
sinv = 1;
cosv = 0;
}
else if (rotate == 180)
{
sinv = 0;
cosv = -1;
}
else if (rotate == 270)
{
sinv = -1;
cosv = 0;
}
else
{
ang = rotate / 180 * Q_PI;
sinv = sin(ang);
cosv = cos(ang);
}
if (vecs[0][0])
sv = 0;
else if (vecs[0][1])
sv = 1;
else
sv = 2;
if (vecs[1][0])
tv = 0;
else if (vecs[1][1])
tv = 1;
else
tv = 2;
for (i = 0 ; i < 2 ; i++)
{
ns = cosv * vecs[i][sv] - sinv * vecs[i][tv];
nt = sinv * vecs[i][sv] + cosv * vecs[i][tv];
vecs[i][sv] = ns;
vecs[i][tv] = nt;
}
for (i = 0 ; i < 2 ; i++)
for (j = 0 ; j < 3 ; j++)
tx.vecs[i][j] = vecs[i][j] / scale[i];
tx.vecs[0][3] = shift[0];
tx.vecs[1][3] = shift[1];
}
// unique the texinfo
f->texinfo = FindTexinfo (&tx);
} while (1);
// JDC 8/8/97
// origin brushes are removed, but they set
// the rotation origin for the rest of the brushes
// in the entity
//
if (q_strncasecmp (name, "origin",6))
{ // keep it
nummapbrushes++;
b->next = mapent->brushes;
mapent->brushes = b;
}
else
{ // don't save the brush, just use as entity origin
char string[32];
vec3_t origin;
if (num_entities == 1)
Error ("Origin brushes not allowed in world");
BrushOrigin (b, origin);
sprintf (string, "%i %i %i", (int)origin[0], (int)origin[1], (int)origin[2]);
SetKeyValue (mapent, "origin", string);
VectorCopy (origin, mapent->origin);
memset (b, 0, sizeof(*b));
}
}
void Yadro::getVerTexArrays() // определить массив вершин и текстурных координат
{
const GLfloat pi=3.141593, k=pi/180; // глобальная переменная
GLfloat R=1.2f; // радиус сферы
GLfloat alpha=pi/2;
GLfloat r = 0.1f; // радиус сферы
// вычисляем точки
//0 точка
VertexArrayYadro[0][0]=0.4+0.0f*r; // x
VertexArrayYadro[0][1]=0.4+0.0f*r; // y
VertexArrayYadro[0][2]=0.4+R*r; // z
//
//1 точка
VertexArrayYadro[1][1]=0.4+R*sin(alpha)*sin(0)*r;
VertexArrayYadro[1][0]=0.4+R*sin(alpha)*cos(0)*r;
VertexArrayYadro[1][2]=0.4+R*cos(alpha)*r;
//
//2 точка
VertexArrayYadro[2][1]=0.4+R*sin(alpha)*sin(pi/2)*r;
VertexArrayYadro[2][0]=0.4+R*sin(alpha)*cos(pi/2)*r;
VertexArrayYadro[2][2]=0.4+R* cos(alpha)*r;
//
//3 точка
VertexArrayYadro[3][1]=0.4+R*sin(alpha)*sin(pi)*r;
VertexArrayYadro[3][0]=0.4+R*sin(alpha)*cos(pi)*r;
VertexArrayYadro[3][2]=0.4+R* cos(alpha)*r;
//
//4 точка
VertexArrayYadro[4][1]=0.4+R*sin(alpha)*sin(3*pi/2)*r;
VertexArrayYadro[4][0]=0.4+R*sin(alpha)*cos(3*pi/2)*r;
VertexArrayYadro[4][2]=0.4+R*cos(alpha)*r;
//
//5 точка
VertexArrayYadro[5][1]=0.4+0.0f*r;
VertexArrayYadro[5][0]=0.4+0.0f*r;
VertexArrayYadro[5][2]=0.4-R*r;
TextureArray[0][0]=0.5f*r; // x (s)
TextureArray[0][1]=1.0f*r; // y (t)
TextureArray[1][0]=1.0f*r;
TextureArray[1][1]=0.0f*r;
TextureArray[2][0]=0.0f*r;
TextureArray[2][1]=0.0f*r;
TextureArray[3][0]=1.0f*r;
TextureArray[3][1]=0.0f*r;
TextureArray[4][0]=0.0f*r;
TextureArray[4][1]=0.0f*r;
TextureArray[5][0]=0.5f*r;
TextureArray[5][1]=1.0f*r;
}
void cEqAppui_PProjInc_M2CPolyn2::ComputeValDeriv()
{
double tmp0_ = mCompCoord[9];
double tmp1_ = mCompCoord[10];
double tmp2_ = cos(tmp1_);
double tmp3_ = mCompCoord[15];
double tmp4_ = mCompCoord[16];
double tmp5_ = mCompCoord[17];
double tmp6_ = sin(tmp0_);
double tmp7_ = cos(tmp0_);
double tmp8_ = sin(tmp1_);
double tmp9_ = mCompCoord[11];
double tmp10_ = mLocProjI_x * tmp3_;
double tmp11_ = mLocProjP0_x + tmp10_;
double tmp12_ = mLocProjJ_x * tmp4_;
double tmp13_ = tmp11_ + tmp12_;
double tmp14_ = mLocProjK_x * tmp5_;
double tmp15_ = tmp13_ + tmp14_;
double tmp16_ = mCompCoord[12];
double tmp17_ = (tmp15_) - tmp16_;
double tmp18_ = sin(tmp9_);
double tmp19_ = -(tmp18_);
double tmp20_ = -(tmp8_);
double tmp21_ = cos(tmp9_);
double tmp22_ = mLocProjI_y * tmp3_;
double tmp23_ = mLocProjP0_y + tmp22_;
double tmp24_ = mLocProjJ_y * tmp4_;
double tmp25_ = tmp23_ + tmp24_;
double tmp26_ = mLocProjK_y * tmp5_;
double tmp27_ = tmp25_ + tmp26_;
double tmp28_ = mCompCoord[13];
double tmp29_ = (tmp27_) - tmp28_;
double tmp30_ = mLocProjI_z * tmp3_;
double tmp31_ = mLocProjP0_z + tmp30_;
double tmp32_ = mLocProjJ_z * tmp4_;
double tmp33_ = tmp31_ + tmp32_;
double tmp34_ = mLocProjK_z * tmp5_;
double tmp35_ = tmp33_ + tmp34_;
double tmp36_ = mCompCoord[14];
double tmp37_ = (tmp35_) - tmp36_;
double tmp38_ = -(tmp6_);
double tmp39_ = tmp7_ * tmp20_;
double tmp40_ = tmp6_ * tmp20_;
double tmp41_ = mCompCoord[0];
double tmp42_ = tmp38_ * tmp19_;
double tmp43_ = tmp39_ * tmp21_;
double tmp44_ = tmp42_ + tmp43_;
double tmp45_ = (tmp44_) * (tmp17_);
double tmp46_ = tmp7_ * tmp19_;
double tmp47_ = tmp40_ * tmp21_;
double tmp48_ = tmp46_ + tmp47_;
double tmp49_ = (tmp48_) * (tmp29_);
double tmp50_ = tmp45_ + tmp49_;
double tmp51_ = tmp2_ * tmp21_;
double tmp52_ = tmp51_ * (tmp37_);
double tmp53_ = tmp50_ + tmp52_;
double tmp54_ = tmp41_ / (tmp53_);
double tmp55_ = tmp7_ * tmp2_;
double tmp56_ = tmp55_ * (tmp17_);
double tmp57_ = tmp6_ * tmp2_;
double tmp58_ = tmp57_ * (tmp29_);
double tmp59_ = tmp56_ + tmp58_;
double tmp60_ = tmp8_ * (tmp37_);
double tmp61_ = tmp59_ + tmp60_;
double tmp62_ = (tmp61_) * (tmp54_);
double tmp63_ = mCompCoord[1];
double tmp64_ = tmp62_ + tmp63_;
double tmp65_ = (tmp64_) - mLocPolyn2_State_1_0;
double tmp66_ = (tmp65_) / mLocPolyn2_State_0_0;
double tmp67_ = tmp38_ * tmp21_;
double tmp68_ = tmp39_ * tmp18_;
double tmp69_ = tmp67_ + tmp68_;
double tmp70_ = (tmp69_) * (tmp17_);
double tmp71_ = tmp7_ * tmp21_;
double tmp72_ = tmp40_ * tmp18_;
double tmp73_ = tmp71_ + tmp72_;
double tmp74_ = (tmp73_) * (tmp29_);
double tmp75_ = tmp70_ + tmp74_;
double tmp76_ = tmp2_ * tmp18_;
double tmp77_ = tmp76_ * (tmp37_);
double tmp78_ = tmp75_ + tmp77_;
double tmp79_ = (tmp78_) * (tmp54_);
double tmp80_ = mCompCoord[2];
double tmp81_ = tmp79_ + tmp80_;
double tmp82_ = (tmp81_) - mLocPolyn2_State_2_0;
double tmp83_ = (tmp82_) / mLocPolyn2_State_0_0;
double tmp84_ = mCompCoord[3];
double tmp85_ = 1 + tmp84_;
double tmp86_ = ElSquare(tmp53_);
double tmp87_ = (tmp53_) / tmp86_;
double tmp88_ = ElSquare(mLocPolyn2_State_0_0);
double tmp89_ = mCompCoord[4];
double tmp90_ = (tmp87_) * (tmp61_);
double tmp91_ = tmp90_ * mLocPolyn2_State_0_0;
double tmp92_ = (tmp91_) / tmp88_;
double tmp93_ = (tmp92_) * (tmp66_);
double tmp94_ = mCompCoord[5];
double tmp95_ = tmp94_ * 2;
double tmp96_ = (tmp87_) * (tmp78_);
double tmp97_ = tmp96_ * mLocPolyn2_State_0_0;
double tmp98_ = (tmp97_) / tmp88_;
double tmp99_ = mCompCoord[6];
double tmp100_ = (tmp98_) * (tmp83_);
double tmp101_ = mCompCoord[7];
double tmp102_ = mLocPolyn2_State_0_0 / tmp88_;
double tmp103_ = (tmp102_) * (tmp66_);
double tmp104_ = (tmp102_) * (tmp83_);
double tmp105_ = (tmp66_) * (tmp66_);
double tmp106_ = (tmp66_) * (tmp83_);
double tmp107_ = (tmp83_) * (tmp83_);
double tmp108_ = -(1);
double tmp109_ = tmp108_ * tmp6_;
double tmp110_ = -(tmp7_);
double tmp111_ = tmp109_ * tmp20_;
double tmp112_ = tmp110_ * tmp19_;
double tmp113_ = tmp111_ * tmp21_;
double tmp114_ = tmp112_ + tmp113_;
double tmp115_ = (tmp114_) * (tmp17_);
double tmp116_ = tmp109_ * tmp19_;
double tmp117_ = tmp116_ + tmp43_;
double tmp118_ = (tmp117_) * (tmp29_);
double tmp119_ = tmp115_ + tmp118_;
double tmp120_ = tmp41_ * (tmp119_);
double tmp121_ = -(tmp120_);
double tmp122_ = tmp121_ / tmp86_;
double tmp123_ = tmp109_ * tmp2_;
double tmp124_ = tmp123_ * (tmp17_);
double tmp125_ = tmp55_ * (tmp29_);
double tmp126_ = tmp124_ + tmp125_;
double tmp127_ = (tmp126_) * (tmp54_);
double tmp128_ = (tmp122_) * (tmp61_);
double tmp129_ = tmp127_ + tmp128_;
double tmp130_ = (tmp129_) * mLocPolyn2_State_0_0;
double tmp131_ = (tmp130_) / tmp88_;
double tmp132_ = (tmp131_) * (tmp66_);
double tmp133_ = tmp110_ * tmp21_;
double tmp134_ = tmp111_ * tmp18_;
double tmp135_ = tmp133_ + tmp134_;
double tmp136_ = (tmp135_) * (tmp17_);
double tmp137_ = tmp109_ * tmp21_;
double tmp138_ = tmp137_ + tmp68_;
double tmp139_ = (tmp138_) * (tmp29_);
double tmp140_ = tmp136_ + tmp139_;
double tmp141_ = (tmp140_) * (tmp54_);
double tmp142_ = (tmp122_) * (tmp78_);
double tmp143_ = tmp141_ + tmp142_;
double tmp144_ = (tmp143_) * mLocPolyn2_State_0_0;
double tmp145_ = (tmp144_) / tmp88_;
double tmp146_ = (tmp145_) * (tmp83_);
double tmp147_ = tmp108_ * tmp8_;
double tmp148_ = -(tmp2_);
double tmp149_ = tmp148_ * tmp7_;
double tmp150_ = tmp148_ * tmp6_;
double tmp151_ = tmp149_ * tmp21_;
double tmp152_ = tmp151_ * (tmp17_);
double tmp153_ = tmp150_ * tmp21_;
double tmp154_ = tmp153_ * (tmp29_);
double tmp155_ = tmp152_ + tmp154_;
double tmp156_ = tmp147_ * tmp21_;
double tmp157_ = tmp156_ * (tmp37_);
double tmp158_ = tmp155_ + tmp157_;
double tmp159_ = tmp41_ * (tmp158_);
double tmp160_ = -(tmp159_);
double tmp161_ = tmp160_ / tmp86_;
double tmp162_ = tmp147_ * tmp7_;
double tmp163_ = tmp162_ * (tmp17_);
double tmp164_ = tmp147_ * tmp6_;
double tmp165_ = tmp164_ * (tmp29_);
double tmp166_ = tmp163_ + tmp165_;
double tmp167_ = tmp2_ * (tmp37_);
double tmp168_ = tmp166_ + tmp167_;
double tmp169_ = (tmp168_) * (tmp54_);
double tmp170_ = (tmp161_) * (tmp61_);
double tmp171_ = tmp169_ + tmp170_;
double tmp172_ = (tmp171_) * mLocPolyn2_State_0_0;
double tmp173_ = (tmp172_) / tmp88_;
double tmp174_ = (tmp173_) * (tmp66_);
double tmp175_ = tmp149_ * tmp18_;
double tmp176_ = tmp175_ * (tmp17_);
double tmp177_ = tmp150_ * tmp18_;
double tmp178_ = tmp177_ * (tmp29_);
double tmp179_ = tmp176_ + tmp178_;
double tmp180_ = tmp147_ * tmp18_;
double tmp181_ = tmp180_ * (tmp37_);
double tmp182_ = tmp179_ + tmp181_;
double tmp183_ = (tmp182_) * (tmp54_);
double tmp184_ = (tmp161_) * (tmp78_);
double tmp185_ = tmp183_ + tmp184_;
double tmp186_ = (tmp185_) * mLocPolyn2_State_0_0;
double tmp187_ = (tmp186_) / tmp88_;
double tmp188_ = (tmp187_) * (tmp83_);
double tmp189_ = -(tmp21_);
double tmp190_ = tmp108_ * tmp18_;
double tmp191_ = tmp189_ * tmp38_;
double tmp192_ = tmp190_ * tmp39_;
double tmp193_ = tmp191_ + tmp192_;
double tmp194_ = (tmp193_) * (tmp17_);
double tmp195_ = tmp189_ * tmp7_;
double tmp196_ = tmp190_ * tmp40_;
double tmp197_ = tmp195_ + tmp196_;
double tmp198_ = (tmp197_) * (tmp29_);
double tmp199_ = tmp194_ + tmp198_;
double tmp200_ = tmp190_ * tmp2_;
double tmp201_ = tmp200_ * (tmp37_);
double tmp202_ = tmp199_ + tmp201_;
double tmp203_ = tmp41_ * (tmp202_);
double tmp204_ = -(tmp203_);
double tmp205_ = tmp204_ / tmp86_;
double tmp206_ = (tmp205_) * (tmp61_);
double tmp207_ = tmp206_ * mLocPolyn2_State_0_0;
double tmp208_ = (tmp207_) / tmp88_;
double tmp209_ = (tmp208_) * (tmp66_);
double tmp210_ = tmp190_ * tmp38_;
double tmp211_ = tmp21_ * tmp39_;
double tmp212_ = tmp210_ + tmp211_;
double tmp213_ = (tmp212_) * (tmp17_);
double tmp214_ = tmp190_ * tmp7_;
double tmp215_ = tmp21_ * tmp40_;
double tmp216_ = tmp214_ + tmp215_;
double tmp217_ = (tmp216_) * (tmp29_);
double tmp218_ = tmp213_ + tmp217_;
double tmp219_ = tmp21_ * tmp2_;
double tmp220_ = tmp219_ * (tmp37_);
double tmp221_ = tmp218_ + tmp220_;
double tmp222_ = (tmp221_) * (tmp54_);
double tmp223_ = (tmp205_) * (tmp78_);
double tmp224_ = tmp222_ + tmp223_;
double tmp225_ = (tmp224_) * mLocPolyn2_State_0_0;
double tmp226_ = (tmp225_) / tmp88_;
double tmp227_ = (tmp226_) * (tmp83_);
double tmp228_ = tmp108_ * (tmp44_);
double tmp229_ = tmp41_ * tmp228_;
double tmp230_ = -(tmp229_);
double tmp231_ = tmp230_ / tmp86_;
double tmp232_ = tmp108_ * tmp55_;
double tmp233_ = tmp232_ * (tmp54_);
double tmp234_ = (tmp231_) * (tmp61_);
double tmp235_ = tmp233_ + tmp234_;
double tmp236_ = (tmp235_) * mLocPolyn2_State_0_0;
double tmp237_ = (tmp236_) / tmp88_;
double tmp238_ = (tmp237_) * (tmp66_);
double tmp239_ = tmp108_ * (tmp69_);
double tmp240_ = tmp239_ * (tmp54_);
double tmp241_ = (tmp231_) * (tmp78_);
double tmp242_ = tmp240_ + tmp241_;
double tmp243_ = (tmp242_) * mLocPolyn2_State_0_0;
double tmp244_ = (tmp243_) / tmp88_;
double tmp245_ = (tmp244_) * (tmp83_);
double tmp246_ = tmp108_ * (tmp48_);
double tmp247_ = tmp41_ * tmp246_;
double tmp248_ = -(tmp247_);
double tmp249_ = tmp248_ / tmp86_;
double tmp250_ = tmp108_ * tmp57_;
double tmp251_ = tmp250_ * (tmp54_);
double tmp252_ = (tmp249_) * (tmp61_);
double tmp253_ = tmp251_ + tmp252_;
double tmp254_ = (tmp253_) * mLocPolyn2_State_0_0;
double tmp255_ = (tmp254_) / tmp88_;
double tmp256_ = (tmp255_) * (tmp66_);
double tmp257_ = tmp108_ * (tmp73_);
double tmp258_ = tmp257_ * (tmp54_);
double tmp259_ = (tmp249_) * (tmp78_);
double tmp260_ = tmp258_ + tmp259_;
double tmp261_ = (tmp260_) * mLocPolyn2_State_0_0;
double tmp262_ = (tmp261_) / tmp88_;
double tmp263_ = (tmp262_) * (tmp83_);
double tmp264_ = tmp108_ * tmp51_;
double tmp265_ = tmp41_ * tmp264_;
double tmp266_ = -(tmp265_);
double tmp267_ = tmp266_ / tmp86_;
double tmp268_ = tmp147_ * (tmp54_);
double tmp269_ = (tmp267_) * (tmp61_);
double tmp270_ = tmp268_ + tmp269_;
double tmp271_ = (tmp270_) * mLocPolyn2_State_0_0;
double tmp272_ = (tmp271_) / tmp88_;
double tmp273_ = (tmp272_) * (tmp66_);
double tmp274_ = tmp108_ * tmp76_;
double tmp275_ = tmp274_ * (tmp54_);
double tmp276_ = (tmp267_) * (tmp78_);
double tmp277_ = tmp275_ + tmp276_;
double tmp278_ = (tmp277_) * mLocPolyn2_State_0_0;
double tmp279_ = (tmp278_) / tmp88_;
double tmp280_ = (tmp279_) * (tmp83_);
double tmp281_ = mLocProjI_x * (tmp44_);
double tmp282_ = mLocProjI_y * (tmp48_);
double tmp283_ = tmp281_ + tmp282_;
double tmp284_ = mLocProjI_z * tmp51_;
double tmp285_ = tmp283_ + tmp284_;
double tmp286_ = tmp41_ * (tmp285_);
double tmp287_ = -(tmp286_);
double tmp288_ = tmp287_ / tmp86_;
double tmp289_ = mLocProjI_x * tmp55_;
double tmp290_ = mLocProjI_y * tmp57_;
double tmp291_ = tmp289_ + tmp290_;
double tmp292_ = mLocProjI_z * tmp8_;
double tmp293_ = tmp291_ + tmp292_;
double tmp294_ = (tmp293_) * (tmp54_);
double tmp295_ = (tmp288_) * (tmp61_);
double tmp296_ = tmp294_ + tmp295_;
double tmp297_ = (tmp296_) * mLocPolyn2_State_0_0;
double tmp298_ = (tmp297_) / tmp88_;
double tmp299_ = (tmp298_) * (tmp66_);
double tmp300_ = mLocProjI_x * (tmp69_);
double tmp301_ = mLocProjI_y * (tmp73_);
double tmp302_ = tmp300_ + tmp301_;
double tmp303_ = mLocProjI_z * tmp76_;
double tmp304_ = tmp302_ + tmp303_;
double tmp305_ = (tmp304_) * (tmp54_);
double tmp306_ = (tmp288_) * (tmp78_);
double tmp307_ = tmp305_ + tmp306_;
double tmp308_ = (tmp307_) * mLocPolyn2_State_0_0;
double tmp309_ = (tmp308_) / tmp88_;
double tmp310_ = (tmp309_) * (tmp83_);
double tmp311_ = mLocProjJ_x * (tmp44_);
double tmp312_ = mLocProjJ_y * (tmp48_);
double tmp313_ = tmp311_ + tmp312_;
double tmp314_ = mLocProjJ_z * tmp51_;
double tmp315_ = tmp313_ + tmp314_;
double tmp316_ = tmp41_ * (tmp315_);
double tmp317_ = -(tmp316_);
double tmp318_ = tmp317_ / tmp86_;
double tmp319_ = mLocProjJ_x * tmp55_;
double tmp320_ = mLocProjJ_y * tmp57_;
double tmp321_ = tmp319_ + tmp320_;
double tmp322_ = mLocProjJ_z * tmp8_;
double tmp323_ = tmp321_ + tmp322_;
double tmp324_ = (tmp323_) * (tmp54_);
double tmp325_ = (tmp318_) * (tmp61_);
double tmp326_ = tmp324_ + tmp325_;
double tmp327_ = (tmp326_) * mLocPolyn2_State_0_0;
double tmp328_ = (tmp327_) / tmp88_;
double tmp329_ = (tmp328_) * (tmp66_);
double tmp330_ = mLocProjJ_x * (tmp69_);
double tmp331_ = mLocProjJ_y * (tmp73_);
double tmp332_ = tmp330_ + tmp331_;
double tmp333_ = mLocProjJ_z * tmp76_;
double tmp334_ = tmp332_ + tmp333_;
double tmp335_ = (tmp334_) * (tmp54_);
double tmp336_ = (tmp318_) * (tmp78_);
double tmp337_ = tmp335_ + tmp336_;
double tmp338_ = (tmp337_) * mLocPolyn2_State_0_0;
double tmp339_ = (tmp338_) / tmp88_;
double tmp340_ = (tmp339_) * (tmp83_);
double tmp341_ = mLocProjK_x * (tmp44_);
double tmp342_ = mLocProjK_y * (tmp48_);
double tmp343_ = tmp341_ + tmp342_;
double tmp344_ = mLocProjK_z * tmp51_;
double tmp345_ = tmp343_ + tmp344_;
double tmp346_ = tmp41_ * (tmp345_);
double tmp347_ = -(tmp346_);
double tmp348_ = tmp347_ / tmp86_;
double tmp349_ = mLocProjK_x * tmp55_;
double tmp350_ = mLocProjK_y * tmp57_;
double tmp351_ = tmp349_ + tmp350_;
double tmp352_ = mLocProjK_z * tmp8_;
double tmp353_ = tmp351_ + tmp352_;
double tmp354_ = (tmp353_) * (tmp54_);
double tmp355_ = (tmp348_) * (tmp61_);
double tmp356_ = tmp354_ + tmp355_;
double tmp357_ = (tmp356_) * mLocPolyn2_State_0_0;
double tmp358_ = (tmp357_) / tmp88_;
double tmp359_ = (tmp358_) * (tmp66_);
double tmp360_ = mLocProjK_x * (tmp69_);
double tmp361_ = mLocProjK_y * (tmp73_);
double tmp362_ = tmp360_ + tmp361_;
double tmp363_ = mLocProjK_z * tmp76_;
double tmp364_ = tmp362_ + tmp363_;
double tmp365_ = (tmp364_) * (tmp54_);
double tmp366_ = (tmp348_) * (tmp78_);
double tmp367_ = tmp365_ + tmp366_;
double tmp368_ = (tmp367_) * mLocPolyn2_State_0_0;
double tmp369_ = (tmp368_) / tmp88_;
double tmp370_ = (tmp369_) * (tmp83_);
double tmp371_ = 1 - tmp84_;
double tmp372_ = (tmp92_) * (tmp83_);
double tmp373_ = (tmp98_) * (tmp66_);
double tmp374_ = tmp372_ + tmp373_;
double tmp375_ = tmp100_ + tmp100_;
double tmp376_ = tmp99_ * 2;
double tmp377_ = tmp93_ + tmp93_;
double tmp378_ = mCompCoord[8];
double tmp379_ = (tmp102_) * tmp89_;
double tmp380_ = tmp103_ + tmp103_;
double tmp381_ = tmp104_ + tmp104_;
double tmp382_ = (tmp66_) * mLocPolyn2_State_0_0;
double tmp383_ = tmp382_ * mLocScNorm;
double tmp384_ = tmp106_ * mLocPolyn2_State_0_0;
double tmp385_ = tmp384_ * mLocScNorm;
double tmp386_ = (tmp131_) * (tmp83_);
double tmp387_ = (tmp145_) * (tmp66_);
double tmp388_ = tmp386_ + tmp387_;
double tmp389_ = tmp146_ + tmp146_;
double tmp390_ = tmp132_ + tmp132_;
double tmp391_ = (tmp173_) * (tmp83_);
double tmp392_ = (tmp187_) * (tmp66_);
double tmp393_ = tmp391_ + tmp392_;
double tmp394_ = tmp188_ + tmp188_;
double tmp395_ = tmp174_ + tmp174_;
double tmp396_ = (tmp208_) * (tmp83_);
double tmp397_ = (tmp226_) * (tmp66_);
double tmp398_ = tmp396_ + tmp397_;
double tmp399_ = tmp227_ + tmp227_;
double tmp400_ = tmp209_ + tmp209_;
double tmp401_ = (tmp237_) * (tmp83_);
double tmp402_ = (tmp244_) * (tmp66_);
double tmp403_ = tmp401_ + tmp402_;
double tmp404_ = tmp245_ + tmp245_;
double tmp405_ = tmp238_ + tmp238_;
double tmp406_ = (tmp255_) * (tmp83_);
double tmp407_ = (tmp262_) * (tmp66_);
double tmp408_ = tmp406_ + tmp407_;
double tmp409_ = tmp263_ + tmp263_;
double tmp410_ = tmp256_ + tmp256_;
double tmp411_ = (tmp272_) * (tmp83_);
double tmp412_ = (tmp279_) * (tmp66_);
double tmp413_ = tmp411_ + tmp412_;
double tmp414_ = tmp280_ + tmp280_;
double tmp415_ = tmp273_ + tmp273_;
double tmp416_ = (tmp298_) * (tmp83_);
double tmp417_ = (tmp309_) * (tmp66_);
double tmp418_ = tmp416_ + tmp417_;
double tmp419_ = tmp310_ + tmp310_;
double tmp420_ = tmp299_ + tmp299_;
double tmp421_ = (tmp328_) * (tmp83_);
double tmp422_ = (tmp339_) * (tmp66_);
double tmp423_ = tmp421_ + tmp422_;
double tmp424_ = tmp340_ + tmp340_;
double tmp425_ = tmp329_ + tmp329_;
double tmp426_ = (tmp358_) * (tmp83_);
double tmp427_ = (tmp369_) * (tmp66_);
double tmp428_ = tmp426_ + tmp427_;
double tmp429_ = tmp370_ + tmp370_;
double tmp430_ = tmp359_ + tmp359_;
mVal[0] = ((mLocPolyn2_State_1_0 + (((tmp85_) * (tmp66_) + tmp89_ * (tmp83_)) - tmp95_ * tmp105_ + tmp99_ * tmp106_ + tmp101_ * tmp107_) * mLocPolyn2_State_0_0) - mLocXIm) * mLocScNorm;
mCompDer[0][0] = (((tmp92_) * (tmp85_) + (tmp98_) * tmp89_) - (tmp377_) * tmp95_ + (tmp374_) * tmp99_ + (tmp375_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][1] = ((tmp102_) * (tmp85_) - (tmp380_) * tmp95_ + tmp104_ * tmp99_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][2] = (tmp379_ + tmp103_ * tmp99_ + (tmp381_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][3] = tmp383_;
mCompDer[0][4] = (tmp83_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][5] = -(2 * tmp105_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][6] = tmp385_;
mCompDer[0][7] = tmp107_ * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][8] = 0;
mCompDer[0][9] = (((tmp131_) * (tmp85_) + (tmp145_) * tmp89_) - (tmp390_) * tmp95_ + (tmp388_) * tmp99_ + (tmp389_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][10] = (((tmp173_) * (tmp85_) + (tmp187_) * tmp89_) - (tmp395_) * tmp95_ + (tmp393_) * tmp99_ + (tmp394_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][11] = (((tmp208_) * (tmp85_) + (tmp226_) * tmp89_) - (tmp400_) * tmp95_ + (tmp398_) * tmp99_ + (tmp399_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][12] = (((tmp237_) * (tmp85_) + (tmp244_) * tmp89_) - (tmp405_) * tmp95_ + (tmp403_) * tmp99_ + (tmp404_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][13] = (((tmp255_) * (tmp85_) + (tmp262_) * tmp89_) - (tmp410_) * tmp95_ + (tmp408_) * tmp99_ + (tmp409_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][14] = (((tmp272_) * (tmp85_) + (tmp279_) * tmp89_) - (tmp415_) * tmp95_ + (tmp413_) * tmp99_ + (tmp414_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][15] = (((tmp298_) * (tmp85_) + (tmp309_) * tmp89_) - (tmp420_) * tmp95_ + (tmp418_) * tmp99_ + (tmp419_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][16] = (((tmp328_) * (tmp85_) + (tmp339_) * tmp89_) - (tmp425_) * tmp95_ + (tmp423_) * tmp99_ + (tmp424_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[0][17] = (((tmp358_) * (tmp85_) + (tmp369_) * tmp89_) - (tmp430_) * tmp95_ + (tmp428_) * tmp99_ + (tmp429_) * tmp101_) * mLocPolyn2_State_0_0 * mLocScNorm;
mVal[1] = ((mLocPolyn2_State_2_0 + (((tmp371_) * (tmp83_) + tmp89_ * (tmp66_) + tmp94_ * tmp106_) - tmp376_ * tmp107_ + tmp378_ * tmp105_) * mLocPolyn2_State_0_0) - mLocYIm) * mLocScNorm;
mCompDer[1][0] = (((tmp98_) * (tmp371_) + (tmp92_) * tmp89_ + (tmp374_) * tmp94_) - (tmp375_) * tmp376_ + (tmp377_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][1] = (tmp379_ + tmp104_ * tmp94_ + (tmp380_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][2] = (((tmp102_) * (tmp371_) + tmp103_ * tmp94_) - (tmp381_) * tmp376_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][3] = tmp108_ * (tmp83_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][4] = tmp383_;
mCompDer[1][5] = tmp385_;
mCompDer[1][6] = -(2 * tmp107_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][7] = 0;
mCompDer[1][8] = tmp105_ * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][9] = (((tmp145_) * (tmp371_) + (tmp131_) * tmp89_ + (tmp388_) * tmp94_) - (tmp389_) * tmp376_ + (tmp390_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][10] = (((tmp187_) * (tmp371_) + (tmp173_) * tmp89_ + (tmp393_) * tmp94_) - (tmp394_) * tmp376_ + (tmp395_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][11] = (((tmp226_) * (tmp371_) + (tmp208_) * tmp89_ + (tmp398_) * tmp94_) - (tmp399_) * tmp376_ + (tmp400_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][12] = (((tmp244_) * (tmp371_) + (tmp237_) * tmp89_ + (tmp403_) * tmp94_) - (tmp404_) * tmp376_ + (tmp405_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][13] = (((tmp262_) * (tmp371_) + (tmp255_) * tmp89_ + (tmp408_) * tmp94_) - (tmp409_) * tmp376_ + (tmp410_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][14] = (((tmp279_) * (tmp371_) + (tmp272_) * tmp89_ + (tmp413_) * tmp94_) - (tmp414_) * tmp376_ + (tmp415_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][15] = (((tmp309_) * (tmp371_) + (tmp298_) * tmp89_ + (tmp418_) * tmp94_) - (tmp419_) * tmp376_ + (tmp420_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][16] = (((tmp339_) * (tmp371_) + (tmp328_) * tmp89_ + (tmp423_) * tmp94_) - (tmp424_) * tmp376_ + (tmp425_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
mCompDer[1][17] = (((tmp369_) * (tmp371_) + (tmp358_) * tmp89_ + (tmp428_) * tmp94_) - (tmp429_) * tmp376_ + (tmp430_) * tmp378_) * mLocPolyn2_State_0_0 * mLocScNorm;
}