void
grid3d_update (grid3d * g, float angle, float *vals, float dist)
{
int i;
float cosa;
float sina;
surf3d *s = &(g->surf);
v3d cam = s->center;
cam.z += dist;
SINCOS ((angle / 4.3f), sina, cosa);
cam.y += sina * 2.0f;
SINCOS (angle, sina, cosa);
if (g->mode == 0) {
if (vals)
for (i = 0; i < g->defx; i++)
s->vertex[i].y = s->vertex[i].y * 0.2 + vals[i] * 0.8;
for (i = g->defx; i < s->nbvertex; i++) {
s->vertex[i].y *= 0.255f;
s->vertex[i].y += (s->vertex[i - g->defx].y * 0.777f);
}
}
for (i = 0; i < s->nbvertex; i++) {
Y_ROTATE_V3D (s->vertex[i], s->svertex[i], cosa, sina);
TRANSLATE_V3D (cam, s->svertex[i]);
}
}
/// \brief Log: SE3 -> se3.
///
/// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
template <typename _Scalar, int _Options> MotionTpl<_Scalar,_Options>
log6(const SE3Tpl<_Scalar, _Options> & m)
{
typedef _Scalar Scalar;
typedef typename SE3Tpl<Scalar,_Options>::Vector3 Vector3;
typedef typename SE3Tpl<Scalar,_Options>::Matrix3 Matrix3;
const Matrix3 & R = m.rotation();
const Vector3 & p = m.translation();
Vector3 w(log3(R));
Vector3 v;
Scalar t = w.norm();
if (t > 1e-15)
{
Matrix3 S(skew(w));
double ct,st; SINCOS (t,&st,&ct);
Matrix3 V(
Matrix3::Identity() +
(1 - ct) / (t * t) * S + (t - st) / (t * t * t) * S * S);
v = V.inverse() * p;
}
else
{
v = p;
}
return MotionTpl<_Scalar,_Options>(v, w);
}
/// \brief Exp: se3 -> SE3.
///
/// Return the integral of the input spatial velocity during time 1.
template <typename _Scalar, int _Options> SE3Tpl<_Scalar, _Options>
exp6(const MotionTpl<_Scalar,_Options> & nu)
{
typedef _Scalar Scalar;
typedef typename MotionTpl<Scalar,_Options>::Vector3 Vector3;
typedef typename MotionTpl<Scalar,_Options>::Matrix3 Matrix3;
const Vector3 & w = nu.angular();
const Vector3 & v = nu.linear();
Scalar t = w.norm();
if (t > 1e-15)
{
Matrix3 R(exp3(w));
Matrix3 S(skew(w));
double ct,st; SINCOS (t,&st,&ct);
Matrix3 V(
Matrix3::Identity() +
(1 - ct) / (t * t) * S + (t - st) / (t * t * t) * S * S);
Vector3 p(V * v);
return SE3Tpl<_Scalar, _Options>(R, p);
}
else
{
return SE3Tpl<_Scalar, _Options>(Matrix3::Identity(), v);
}
}
void surf3d_rotate (surf3d *s, float angle) {
int i;
float cosa;
float sina;
SINCOS(angle,sina,cosa);
for (i=0;i<s->nbvertex;i++) {
Y_ROTATE_V3D(s->vertex[i],s->svertex[i],cosa,sina);
}
}
/*----------------------------------------------------------*
| <<< サークル描画 >>>
| 入力 iType = 1 : ライン
| = 0 : 塗りつぶし
| iNum = 円のどの位置か
| iDiv = 円の分割数
| vecPos = 表示座標
| r0, r1 = 半径 ※ r0 が内側
| dwCol = 塗りつぶし色
*-----------------------------------------------------------*/
static void CircleDraw(int iType, int iNum, int iDiv, VEC2 * pPos, float r0, float r1, DWORD dwCol)
{
D3_VTX_TL_ vt[4 + 1];
float fR = D3DX_PI / (iDiv / 2.f);
float fZoom = 1.55f;
float s0, c0, s1, c1;
SINCOS(fR * (iDiv - iNum), &s0, &c0);
SINCOS(fR * (iDiv - iNum - 0.6f), &s1, &c1);
vt[0].x = D3_CONV_X(pPos->x + (r0 * c0) * fZoom);
vt[0].y = D3_CONV_Y(pPos->y + r0 * s0);
vt[0].z = 0;
vt[0].rhw = 1.f;
vt[0].c = dwCol;
vt[1].x = D3_CONV_X(pPos->x + (r0 * c1) * fZoom);
vt[1].y = D3_CONV_Y(pPos->y + r0 * s1);
vt[1].z = 0;
vt[1].rhw = 1.f;
vt[1].c = dwCol;
vt[2].x = D3_CONV_X(pPos->x + (r1 * c1) * fZoom);
vt[2].y = D3_CONV_Y(pPos->y + r1 * s1);
vt[2].z = 0;
vt[2].rhw = 1.f;
vt[2].c = dwCol;
vt[3].x = D3_CONV_X(pPos->x + (r1 * c0) * fZoom);
vt[3].y = D3_CONV_Y(pPos->y + r1 * s0);
vt[3].z = 0;
vt[3].rhw = 1.f;
vt[3].c = dwCol;
vt[4] = vt[0];
d3_light_set( false);
d3_stat_blend_set(D3_BLEND_ADD);
d3_tex_set( -1);
d3_material_set( &d3.matNormal);
if(iType == 1){ D3_RENDER(D3DPT_LINESTRIP, vt, TL, 4);}
else { D3_RENDER(D3DPT_TRIANGLEFAN, vt, TL, 4);}
d3_light_set( true);
d3_stat_blend_set(D3_BLEND_NORMAL);
}
double sph_j1c(double q)
{
if (q < SPH_J1C_CUTOFF) {
const double q2 = q*q;
return (1.0 + q2*(-3./30. + q2*(3./840. + q2*(-3./45360.))));// + q2*(3./3991680.)))));
} else {
double sin_q, cos_q;
SINCOS(q, sin_q, cos_q);
return 3.0*(sin_q/q - cos_q)/(q*q);
}
}
void calc (JointData & data,
const Eigen::VectorXd & qs) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
}
double sas_J1(double x)
{
//Cephes double pression function
#if FLOAT_SIZE>4
double w, z, p, q, xn;
const double Z1 = 1.46819706421238932572E1;
const double Z2 = 4.92184563216946036703E1;
const double THPIO4 = 2.35619449019234492885;
const double SQ2OPI = 0.79788456080286535588;
w = x;
if( x < 0 )
w = -x;
if( w <= 5.0 )
{
z = x * x;
w = polevl( z, RPJ1, 3 ) / p1evl( z, RQJ1, 8 );
w = w * x * (z - Z1) * (z - Z2);
return( w );
}
w = 5.0/x;
z = w * w;
p = polevl( z, PPJ1, 6)/polevl( z, PQJ1, 6 );
q = polevl( z, QPJ1, 7)/p1evl( z, QQJ1, 7 );
xn = x - THPIO4;
double sn, cn;
SINCOS(xn, sn, cn);
p = p * cn - w * q * sn;
return( p * SQ2OPI / sqrt(x) );
//Single precission version of cephes
#else
double xx, w, z, p, q, xn;
const double Z1 = 1.46819706421238932572E1;
const double THPIO4F = 2.35619449019234492885; /* 3*pi/4 */
xx = x;
if( xx < 0 )
xx = -x;
if( xx <= 2.0 )
{
z = xx * xx;
p = (z-Z1) * xx * polevl( z, JPJ1, 4 );
return( p );
}
q = 1.0/x;
w = sqrt(q);
p = w * polevl( q, MO1J1, 7);
w = q*q;
xn = q * polevl( w, PH1J1, 7) - THPIO4F;
p = p * cos(xn + xx);
return(p);
#endif
}
