I see that the setRotationMatrix set translation too.
I solve my issue by this class:
public class Object3dEx extends Object3D {
private static final short childMax = 10;
private String group;
private int childCnt = 0;
private Object3dEx[] children = new Object3dEx[childMax];
//Rotation around vector cache
private Matrix cacheRotAxisMat;
private float cacheRotAng;
private float cacheRotAngCos;
private float cacheRotAngSin;
private SimpleVector cacheRotVec;
public Object3dEx(Object3D obj) {
super(obj, true);
}
public String getGroup() {
return group;
}
public void setGroup(String group) {
this.group = group;
}
public void addChildEx(Object3dEx o) {
if (o != null && childCnt < childMax) {
children[childCnt] = o;
++childCnt;
}
}
public void removeChildEx(Object3dEx o) {
if (o != null) {
for (int i = 0; i < children.length; i++) {
if (o.equals(children[i])) {
children[i] = null;
--childCnt;
}
}
}
}
@Override
public void translate(float x, float y, float z) {
translate(new SimpleVector(x, y, z));
}
@Override
public void translate(SimpleVector trans) {
super.translate(trans);
if (children != null && children.length > 0) {
for (Object3dEx ch : children) {
if (ch != null) {
ch.translate(trans);
}
}
}
}
@Override
public void rotateX(float w) {
super.rotateX(w);
rotateChildren(w, 0, 0);
}
@Override
public void rotateY(float w) {
super.rotateY(w);
rotateChildren(0, w, 0);
}
@Override
public void rotateZ(float w) {
super.rotateZ(w);
rotateChildren(0, 0, w);
}
@Override
public void setScale(float absScale) {
super.setScale(absScale);
if (children != null && children.length > 0) {
for (Object3dEx ch : children) {
if (ch != null) {
ch.setScale(absScale);
}
}
}
}
@Override
public void scale(float scale) {
super.scale(scale);
if (children != null && children.length > 0) {
for (Object3dEx ch : children) {
if (ch != null) {
ch.scale(scale);
}
}
}
}
public void removeChildAll() {
children = new Object3dEx[childMax];
childCnt = 0;
}
public void rotateChildren(float x, float y, float z) {
if (children != null && children.length > 0) {
for (Object3dEx ch : children) {
if (ch != null) {
SimpleVector parentPosition = this.getTransformedCenter();
ch.rotateAroundVectorX(parentPosition, x);
ch.rotateAroundVectorY(parentPosition, y);
ch.rotateAroundVectorZ(parentPosition, z);
}
}
}
}
public void rotateAroundVectorX(SimpleVector vector, float angle) {
if (angle == 0) return;
vector.x = 1;
rotateAroundVector(vector, angle);
}
public void rotateAroundVectorY(SimpleVector vector, float angle) {
if (angle == 0) return;
vector.y = 1;
rotateAroundVector(vector, angle);
}
public void rotateAroundVectorZ(SimpleVector vector, float angle) {
if (angle == 0) return;
vector.z = 1;
rotateAroundVector(vector, angle);
}
public void rotateAroundVector(SimpleVector vector, float angle) {
if (angle == 0) return;
float c, s;
Matrix m;
boolean cachedAngle = false;
if (cacheRotAng == angle) {
c = cacheRotAngCos;
s = cacheRotAngSin;
cachedAngle = true;
} else {
cacheRotAng = angle;
c = FloatMath.cos(angle);
cacheRotAngCos = c;
s = FloatMath.sin(angle);
cacheRotAngSin = s;
}
if (cacheRotVec != null && cacheRotVec.equals(vector) && cachedAngle && cacheRotAxisMat != null) {
m = cacheRotAxisMat;
} else {
cacheRotVec = vector;
m = new Matrix();
float a11 = c + (1 - c) * vector.x * vector.x;
float a12 = (1 - c) * vector.x * vector.y - s * vector.z;
float a13 = (1 - c) * vector.z * vector.x + s * vector.y;
m.setRow(0, a11, a12, a13, 0);
float a21 = (1 - c) * vector.x * vector.y + s * vector.z;
float a22 = c + (1 - c) * vector.y * vector.y;
float a23 = (1 - c) * vector.z * vector.y - s * vector.x;
m.setRow(1, a21, a22, a23, 0);
float a31 = (1 - c) * vector.z * vector.x - s * vector.y;
float a32 = (1 - c) * vector.z * vector.y + s * vector.x;
float a33 = c + (1 - c) * vector.z * vector.z;
m.setRow(2, a31, a32, a33, 0);
m.setRow(3, 0, 0, 0, 1);
cacheRotAxisMat = m;
}
Matrix currentTransformMatrix = getWorldTransformation();
currentTransformMatrix.matMul(m);
clearTranslation();
getRotationMatrix().setRow(3, 0, 0, 0, 1);
setRotationMatrix(currentTransformMatrix);
}
}