private static final void VEC_SUM(vec3f v21, DVector3C v2, vec3f v1) { v21.f[0] = (float) (v2.get0() + v1.f[0]); v21.f[1] = (float) (v2.get1() + v1.f[1]); v21.f[2] = (float) (v2.get2() + v1.f[2]); }
/** * Scales the first parameter with the second and then adds the * result to the current vector. * @param v2 Vector * @param d scale * @return this + v2*d */ public final DVector3 addScaled(DVector3C v2, double d) { d0 += v2.get0()*d; d1 += v2.get1()*d; d2 += v2.get2()*d; return this; }
public static void dAddScaledVectors3(DVector3 res, DVector3C a, DVector3C b, double a_scale, double b_scale) { double res_0, res_1, res_2; res_0 = a_scale * a.get0() + b_scale * b.get0(); res_1 = a_scale * a.get1() + b_scale * b.get1(); res_2 = a_scale * a.get2() + b_scale * b.get2(); // Only assign after all the calculations are over to avoid incurring memory aliasing res.set( res_0, res_1, res_2 ); }
/** * Cross product, set a -= b x c. * @param a a * @param b b * @param c c */ public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DVector3C c) { a.add0( -b.get1()*c.get2() + b.get2()*c.get1() ); a.add1( -b.get2()*c.get0() + b.get0()*c.get2() ); a.add2( -b.get0()*c.get1() + b.get1()*c.get0() ); }
@Override public final void setAxis1 (DVector3C a) //TODO use dVector3 { setAxis1(a.get0(), a.get1(), a.get2()); } @Override
@Override public final void setAxis3 (DVector3C a) //TODO use dVector3 { setAxis3(a.get0(), a.get1(), a.get2()); } @Override
private static vec3f DVector3Tovec3f(DVector3C v) { vec3f vf = new vec3f(); vf.f[0] = (float) v.get0(); vf.f[1] = (float) v.get1(); vf.f[2] = (float) v.get2(); return vf; }
@Override public void setAxis1 (final DVector3C a) //TODO use dVector3 { setAxis1 (a.get0(), a.get1(), a.get2()); } @Override
@Override public void setAxis1 (DVector3C a) //TODO use dVector3 { setAxis1(a.get0(), a.get1(), a.get2()); } @Override
private static vec3f DVector3Tovec3f(DVector3C v) { vec3f vf = new vec3f(); vf.f[0] = (float) v.get0(); vf.f[1] = (float) v.get1(); vf.f[2] = (float) v.get2(); return vf; }
/** * Distance between this vector and a. * @param a a * @return distance */ @Override public final double distance(DVector3C a) { double r1 = get0()-a.get0(); double r2 = get1()-a.get1(); double r3 = get2()-a.get2(); return Math.sqrt(r1*r1 + r2*r2 + r3*r3); }
private void dampRotationalMotion (double kd) { DVector3C w = body[0].getAngularVel(); body[0].addTorque (-kd*w.get0(),-kd*w.get1(),-kd*w.get2()); }
/** * Check whether two vectors contains the same values. * Due to Java's polymorphism handling, this method can be much faster than * v.equals(a). * @param a a * @return quality */ @Override public final boolean isEq(DVector3C a) { return get0()==a.get0() && get1()==a.get1() && get2()==a.get2(); }
/** * Return a new vector v0 = v(this) - v2. * @param v2 v2 * @return new vector */ @Override public final DVector3 reSub(DVector3C v2) { return new DVector3( get0() - v2.get0(), get1() - v2.get1(), get2() - v2.get2()); }
public static void dDQfromW (DQuaternion dq, final DVector3C w, final DQuaternionC q) { //dAASSERT (w, q, dq); dq.set(0, 0.5*(- w.get0()*q.get1() - w.get1()*q.get2() - w.get2()*q.get3()) ); dq.set(1, 0.5*( w.get0()*q.get0() + w.get1()*q.get3() - w.get2()*q.get2()) ); dq.set(2, 0.5*(- w.get0()*q.get3() + w.get1()*q.get0() + w.get2()*q.get1()) ); dq.set(3, 0.5*( w.get0()*q.get2() - w.get1()*q.get1() + w.get2()*q.get0()) ); } }
public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
private void dGeomRaySetNoNormalize(DxRay myRay, DVector3C MyPoint, DVector3C MyVector) { myRay._final_posr.pos.set(MyPoint); DMatrix3 R = myRay._final_posr.Rw(); R.set(0, 2, MyVector.get0()); R.set(1, 2, MyVector.get1()); R.set(2, 2, MyVector.get2()); myRay.dGeomMoved(); } //